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ABSTRACT 

This study investigates the utility of rule-based coordination of motion for rough-terrain locomo- 
tion by a hexapod Avalking machine. The logic for generating leg commands is written in Prolog 
while the simulation of the terrain and of the vehicle kinematics, as well as low level on-board 
computer functions, are written in extended Common Lisp. It is found that this approach results 
in code that is much easier to understand and modify than previous motion coordination pro- 
grams written in Pascal. The authors believe that both the methodology and the stepping logic 
presented in this report possess sufficient merit to justify full-scale physical testing in the Adap- 
tive Suspension Vehicle operated under DARPA contract by Ohio State University. 



1. Introduction 



The Adaptive Suspension Vehicle (ASV) is a large six-legged vehicle designed for outdoor 
operation in rough terrain. Limb motion coordination for the ASV is accomplished by an on- 
board computer network involving fifteen standard single-board computers as well as two special 
purpose computers [l, 2]. The software system is hierarchically organized with a clear distinction 
being made between an individual leg control level, a leg motion coordination level, and a body 
motion planning level [3]. Except for the two special purpose computers, the application software 
for the ASV is currently written almost entirely in Pascal. A custom designed real-time operat- 
ing system, written mainly in PL/M, coordinates the functioning of all processes running on the 
various processors of the vehicle computer [4]. The total ASV software system involves some- 
what more than one million bytes of code. 

An important feature of the ASV is its omni-directional motion capability [l, 2] which gives 
it the general maneuverability characteristics of a helicopter. This behavior is achieved by pro- 
viding the operator with a joystick with three major motion axes for control of vehicle forward 
velocity, lateral velocity, and turning velocity respectively [2]. The vehicle control computer 
accepts these commands and synthesizes a sequence of leg movements to produce the desired 
body behavior. It is assisted in this task by information from an optical terrain scanner which 
provides a map of terrain elevation in the immediate vicinity of the vehicle [5], and by force and 
position feedback from each leg. 

Until now, nearly all outdoor experiments with the ASV have made use of a tripod gait in 
which legs are used in two sets of overlapping tripods [6, 7]. This gait was chosen both for its 
relative simplicity and because it is known to be uniquely optimal for high speed straight-line 
locomotion [7, 8, 9]. However, the tripod gait is not well suited to extreme terrain conditions in 
which a significant fraction of the area under a given leg of the ASV may be unsatisfactory for 
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load bearing due to the presence of rocks, holes, obstacles, soft soils, etc. In the latter case, simu- 
lation experiments [10, 11], and initial indoor testing [12], indicate that on-line optimization of 
leg sequencing should give better results. 

Gaits involving real-time optimization of stability or maneuverability in the presence of ter- 
rain constraints are often called free gaits to distinguish them from the periodic gaits used by 
walking machines and animals in less difficult circumstances [13, 14]. Until now, all ASV experi- 
ments with free gaits have used an imperative language (Pascal) to encode stepping algorithms. 
However, because of the logical complexity of free gaits, the use of such a language produces code 
which is very difficult to understand or to change [15] . As a consequence, one of the authors 
adopted functional programming as implemented in Common Lisp for a simulation study of free 
gait algorithms [11]. While this was found to be helpful, and resulted in substantially more com- 
pact code. Common Lisp does not provide support for either object-oriented programming or 
logic programming [16]. Since optimization of stepping requires simulation of vehicle behavior, 
the use of object-oriented programming ought to produce more readable code. Fortunately, 
extended Common Lisp as implemented on Lisp machines possesses an object-oriented facility 
called Flavors [17]. As part of the work described in this report, the program documented in [ll] 
was recoded using Flavors. The result, attached as an appendix, is believed by the authors to be 
much easier to comprehend and modify than the corresponding code of [ll]. 

Although the use of Flavors represents a significant improvement in modularization and ease 
of understanding, the top level of leg motion planning is still difficult to comprehend. Further 
study has led the authors to the conclusion that this part of the ASV control problem fits a logic 
programming paradigm better than any other. That is, the logical conditions for transitions 
between various leg control states are best described by a set of rules [9, 18, 19]. This being the 
case, Prolog [20] was adopted for coding the top level of the coordination algorithm developed in 
this report. The resulting code is at least an order of magnitude shorter than the corresponding 
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Pascal code, and is remarkably easier to understand and modify. 

The remainder of this report presents first a discussion of the mathematical model used in 
this study to simulate terrain and the ASV vehicle. This is followed by a description of the use of 
finite state machines for control of individual legs [18, 19]. Both these controllers and the vehicle 
and terrain models are simulated by the Lisp programs presented in the appendix. The next 
topic discussed is the use of Prolog to realize the free gait coordination algorithm. The report 
concludes with a discussion of the results of this investigation and suggestions for future research. 

2. Vehicle and Terrain Model 

While the vehicle model used in this study is based on the Adaptive Suspension Vehicle, it 
represents only the major vehicle dimensions and components. Specifically, the cabin and the ter- 
rain scanner are omitted from the simulation model, while the geometries of the body and the 
legs are identical to those of the ASV. Therefore, the simulation model is represented by a simple 
six-faced box with each leg drawn as two line segments as shown in Figure 1. The exact vehicle 
dimensional data can be found in other literature [2, 12]. 

The terrain adopted for this study consists of two types of cells. One type, called a permitted 
cell, is able to support the body load when a leg steps on it. The other type, named a forbidden 
cell, is not usable because of unfavorable terrain conditions. A typical terrain example utilized in 
this study is shown in Figure 1. A cell with an "x" mark is a forbidden cell while unmarked cells 
are permitted. As shown in Figure 1, the simulation terrain model is prismatic in nature. That 
is, the terrain height is determined by a function whose value is governed only by distance along 
a specified horizontal direction on the terrain. Forbidden cells on this terrain can be designated 
either manually by an operator or automatically by using a random number generator with a 
threshold chosen to produce a specified ratio between permitted cells and forbidden cells [ll]. 
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The dimensions of each cell are one foot by one foot when projected onto a horizontal plane. This 
size is comparable to that of the feet of the ASV, and is larger than the resolution of the terrain 
scanner [5, 12], 

An overall block diagram of the program developed in this study is shown in Figure 2. This 
entire program is executed on a Symbolics 3650 Lisp machine [11, 21]. Each box shows an object 
that is an instance of a Flavor [17] with the exception of the Free Gait Coordinator which is writ- 
ten in Symbolics Prolog [21]. Like the physical ASV which has nine major parts, namely, body, 
vision sensor, cab, and six legs, the simulation object, ’’ASV” has correspondingly nine com- 
ponent objects, ’’Body”, ’’Vision Sensor”, ’’Joystick”, and ”Legl” through ”Leg6”. These nine 
objects are linked to ”ASV” through a part relation [22]. Each part has its sub-parts, and again 
links to them with a part relation. Differing from the nine major parts which have visible 
corresponding parts in the real ASV, the subparts of the simulation are not physically tangible, 
but are introduced because of their functionality for program development. For example, the 
”Legl” object, which is a part of the ”ASV” object, has six subparts : ”Legl Plan Machine”, 
”Legl Control Machine”, ”Legl Executor”, ”Legl Contact Sensor”, ”Legl Foothold Finder”, and 
”Legl TKM Calculator”. The ”Legl” object binds all of these subparts into one group with the 
part relation. In order to show the above relations among the objects in Figure 2, the six subpart 
objects are drawn under the ”Legl” object. 

Besides the part relation. Figure 2 also shows the hierarchical control structure linking the 
simulation objects. Specifically, communication is restricted between objects in two adjacent lev- 
els by an assumption that upper levels have the right to access status information at lower levels, 
but the latter must receive explicit commands from upper levels to update their internal state. 
For example, when ”ASV”, the vehicle object, needs ”Legl” to support its body, it sends a 
’’Place” decision to ”Legl” and continuously monitors ”Legl” as to whether ”Legl” has started 
to support the body or is in motion to try to reach a foothold. On receiving a ’’Place” decision 
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from sends the *Tlace^^ decision to **Legl Plan Machine” while making observa- 

tions of this machine. This type of message passing to and status observation from subordinates 
continues until the ”Place” decision is accomplished. That is, when the foot of ”Legl” actually 
hits the ground, the contact sensor of ”Legl” detects the event and changes its internal state. 
The state change of ”Legl Contact Sensor” is observed by ”Legl Executor” and by ”Legl Con- 
trol Machine”. In this way, the state change in the lowest level is propagated to higher levels 
until the touch down event arrives at ”ASV ”. 

The joystick object simulates the physical three-axis joystick of the ASV through the use of 
six keys on the simulation computer keyboard to increment or decrement each of the three rates 
controlled by the joystick. These rates are forward velocity^ lateral velocity^ and turn rate^ all in 
body coordinates. The altitude of the vehicle above the terrain and its orientation in roll and 
pitch relative to the terrain are automatically regulated using the algorithms described in [23]. 

While an elementary representation of the vision sensor is included in the appendix, it is not 
used in this study. Rather, as described in the above discussion of terrain, it is assumed that all 
forbidden cells have already been identified by prior terrain analysis. Of course this assumption 
does not represent a physical limitation of the ASV, but as made merely to allow this simulation 
to be focused on control of stepping, rather than on vision. 

In addition to simplification of vision, this simulation also ignores leg mass in order to avoid 
the complexity of computing a center of gravity which moves with respect to the body. More- 
over, all inertial forces are omitted from the simulation. That is, as in most previous simulation 
studies relating to gaits and control of stepping [8, 9, 10, 11, 24, 25, 26], only static stability is 
considered in this study. While this simplification would be serious in high speed locomotion, free 
gaits are most appropriate to low speed traversal of extremely difficult terrain, so the authors do 
not feel that this is a serious limitation on the applicability of the results of this investigation. 



6 



3. Finite State Control of Individual Legs 



The basic approach to leg control used in this research was first proposed by Tomovic and 
McGhee [27], and involves the subdivision of leg motion into a number of discrete states [9, 18, 
19]. In order to describe the application of this concept to the ASV, a number of definitions are 
needed as follows: 

Definition 1: A foothold is a point on a piece of terrain, and can be assigned to a leg while the leg 
is in the air. When the foot of a leg is placed on the terrain, its assigned foothold becomes the 
support point of the leg. A foothold associated with a leg can be changed to a new one before the 
foothold becomes a support point [10]. 

Definition 2: The support pattern associated with a given set of leg support points is the convex 
hull of any point set in a horizontal plane which contains the vertical projections of all support 
points [13]. 

Definition 3: The magnitude of the stability margin at time t for an arbitrary support pattern is 
equal to the shortest distance from the vertical projection of the vehicle center of gravity to any 
point on the boundary of the support pattern. If the pattern is statically stable, the stability 
margin is positive. Otherwise, it is not defined [15]. 

Definition 4: A working volume is associated with each leg. This volume is a subset of three- 
dimensional space defined relative to the body and consists of the collection of points which can 
be reached by the foot of the given leg [ll, 24]. 

Definition 5: A temporal kinematic margin is associated with each foothold. At any instant, this 
margin is the time remaining until the associated leg would reach the boundary of its working 
volume if the foothold were used as a support point [15, 24]. 
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Evidently, supporting legs have their support points, and these points define a support pattern. 
The static stability of a walking machine is determined by the location of the center of gravity of 
its body with respect to the support pattern. The walking machine can change its body position 
only while the temporal kinematic margins of all supporting legs are larger than zero and the sta- 
bility margin of the walking machine is positive. Therefore, the mobility of the walking machine 
is limited when either its stability is small or the temporal kinematic margin of any leg is small. 
This problem can be corrected by changing the support pattern or by changing the center of grav- 
ity of the machine; i.e., by leg placing and lifting or by body movement. The work presented in 
this report adopts both correction methods to enhance the mobility of a walking machine. 

Because of the unevenness and obstacles associated with rough terrain, in this study leg 
movements during placing and lifting are restricted to be parallel to the direction of gravity in 
order to eliminate the possibility of striking obstacles with the side of the leg. From this con- 
sideration, the trajectory shown in Figure 3 is used for the free gaits discussed in this report. The 
seven segments of this trajectory define the states of the ’’Leg Control Machine” shown in Figure 
4. Among these seven states, three states, ’’Ready”, ’’Descent”, and ’’Support” are asynchronous 
while the remaining four states, ’’Advance”, ’’Contact”, ’’Lift”, and ’’Return” are synchronous. 
Among the three asynchronous states, two states, ’’Ready” and ’’Support”, are introduced to 
handle the variable timing of lifting and placing events necessarily associated with free gaits. 
These two states are terminated by explicit commands from a higher level state machine, called 
the ’’Leg Plan Machine”, shown in Figure 5. On the other hand, the ’’Descent” state of Figure 4 
is designated as an asynchronous state for the reason that the time required for leg contact with 
the ground can be expected to deviate slightly from a nominal value, assumed to be 0.4 seconds, 
because of inaccuracies in measurement or mechanical errors in leg movement. 

In the other four states, ’’Advance”, ’’Contact”, ’’Lift”, and ’’Return”, the accuracy of leg 
movement along the leg trajectory is not so critical as it is in the ’’Descent” state, so slight 
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position errors are acceptable. Thus, these four states are realized as synchronous states. More- 
over, leg movement control based on only a timing event simplifies the control scheme. The 
quantities Tl, T2, T3, and T4 shown in Figure 4 are the time duration of the four synchronous 
states, and represent reasonable amounts of time for the physical ASV leg to finish the associated 
movements. 

As shown in Figures 4 and 5, the plan machines can send either a "Deploy command" or a 
"Recoverjcommand" to their subordinate leg control machines and monitor state changes of the 
latter. On the other hand, as shown in Figure 2, the plan machines are controlled and monitored 
by the leg objects. Thus, the role of the plan machines in the control hierarchy is to buffer the 
decisions of the free gait coordinator to allow for delays between leg motion planning and leg 
motion execution. Specifically, there is a time difference of one second between the leg motion 
planning done by the coordinator and the leg motion execution done by the leg executor under 
the control of the leg control machine. This is necessary because restoration of vehicle stability 
by placing a leg on the ground cannot be accomplished until a designated leg has moved to a 
designated foothold and physically begins to support the body. This prediction time represents 
the nominal time needed by a leg for such an action. Though leg sequencing could be planned a 
longer time ahead, this would require more computation and also would be subject to greater 
inaccuracy because of errors in predicting commands from the operator. Specifically, in this 
study, predicted body motion is derived from an assumption that the desired input commanded 
by an operator will not change within the next second. To make this assumption more reason- 
able, operator commands are filtered with a low pass filter before they are used by the program. 
This action also represents a simple approximation to the physical effect of ASV body inertia, 
since it prevents step changes in vehicle velocity. 

Because the "Leg Plan Machine" is a Moore machine, as shown in Figure 5, labels on arcs 
from state to state are transition conditions, while arrows not terminating on states represent 
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outputs. The two outputs, ”Deploy_command” and ”Recover_command”, are generated from 
the ’’Planned contact” and ’’Actual lift” states respectively, and are given to the lower level con- 
trol machine. After sending out the ’’Deploy command”, the ’’Leg Plan Machine” continuously 
monitors the state change of its subordinate control machine. When the leg physically touches 
the ground, the control machine makes a state transition to the ’’Contact” state. The leg plan 
machine detects the control state transition, and makes its state transition to the ’’Eligible to 
lift” state. If the ’’Descent” control state lasts 0.4 seconds eis planned by the control machine, 
then since the synchronous state ’’Advance” lasts 0.6 seconds, the ’’Planned contact” state of the 
plan machine lasts one second. Thus, the one second projection time is spent while the plan 
machine waits in the ’’Planned contact” state, and the plan machine is thereby synchronized with 
the physical touch-down event. 

The ’’Eligible to lift” state means that the leg associated with the plan machine is ready to 
be lifted from the ground. The plan state transition to the ’’Eligible to lift” state is monitored 
through the leg object, and the information is collected by ”ASV” so that it can provide the coor- 
dinator with the information that the leg is liftable from the ground. Thus, such a leg can be 
removed from the projected support pattern whenever the coordinator decides to do. Such a 
removal can occur in two different ways. One way is simple leg lifting. Whenever a leg is found 
to be redundant to making the vehicle stable, the coordinator can cause that leg to be lifted from 
the ground. The other possibility is that when the coordinator removes a given leg, it adds 
another leg into the projected support pattern. The latter case is called ’’leg exchange”, and is 
intended to improve the mobility of the vehicle at the cost of a small increase in the complexity 
of the plan machines. Specifically, this action is useful when three legs are in a support pattern, 
but one leg is almost at its kinematic limit. In such a situation, the leg near its kinematic limit 
cannot be simply deleted from the support pattern because the vehicle would be unstable. On the 
other hand, without lifting the leg from the ground, the vehicle would soon come to stop because 
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of the leg reaching its kinematic limit. One way to solve this problem is to use two legs instead of 
one leg. That is, to cause one leg selected from the legs available to the coordinator to support 
the body, and at the same time to cause the other leg near its kinematic limit to be lifted from 
the ground. These two actions are performed simultaneously inside the coordinator, but two dis- 
tinct decisions are sent to corresponding plan machines. First, the "Exchange_decision” is sent 
to the plan machine of the leg to be lifted so that the plan state is changed to ^’Planned 
exchange”. Second, the ”Place_decision” is given to the plan machine of the other leg to be 
placed so that the plan state is changed to ^Planned contact”. This action causes generation of a 
”Deploy_command” which is sent to the control machine so that the physical leg starts to move 
and eventually supports the body. The former leg plan machine will wait until it receives from 
its leg object the ”Interlock confirm” signal that Is generated by the ”ASV” when the physical 
leg moving toward the designated supporting point actually touches down on the ground. After 
arrival of the confirm signal, the plan machine changes its state to the ”Actual lift” state, gen- 
erates a ”Recover_command”, and sends the command to the lower level control machine. On 
receiving the command, the control machine changes its state to the "Lift” state, and passes its 
state to the lower level leg executor causing it to perform a physical leg lifting action. 

Besides the "Interlockjconfirm” signal, the "Support jstate” signal from the control machine 
of the leg is also tested before the above state transition of the plan machine is made since a leg 
can be lifted from the ground only when its control state is the "Support” state. The last test in 
the plan state transitions is ”Stable_without" which examines vehicle stability resulting from the 
leg lifting action. Since it is performed one second in advance, this test eliminates the possibility 
that the vehicle could become momentarily unstable because of lifting a leg. 

Referring to Figure 5, the "Actual lift” state of the plan machine can be entered either from 
the "Planned exchange” state or from the "Planned lift” state. The first case is discussed above. 
For the latter state transition, two conditions are tested. One is whether the leg planned to be 
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lifted from the ground is in the ’’Support” control state, and the other is whether the leg can be 
lifted without making the vehicle unstable. The first test ensures a correct control state transition 
to the ’’Lift” control state. The second test takes care of the time difference between the leg lift- 
ing decision from the coordinator and the physical leg lifting action. 

4. Leg Coordination Logic 

The free gait coordinator located at the top of the control hierarchy continuously monitors 
the internal status of the robot object ”ASV”, and sends to it various commands to control the 
body and legs. In carrying out this action, the coordinator uses a free gait strategy which tends to 
maximize the number of legs in the air so long as the robot object is stable. This is done to 
increase the likelihood that the coordinator can find new legs to support its body when the robot 
mobility is limited either because of a small stability margin of its body, or because of a small 
temporal kinematic margin of one of the supporting legs. Thus, the role of the coordinator is 
similar to that of the brain of a horse carrying a human on its back when it walks over rough ter- 
rain. Like a horse that resists or modifies commands from a human operator under difficult con- 
ditions, the coordinator makes the robot object resist or modify operator commands depending on 
leg states, body attitude, and terrain conditions. Specifically, when one of the vehicle supporting 
legs has a small temporal kinematic margin, the coordinator resists the vehicle speed command 
from a human operator by reducing the vehicle speed in order to provide more time for leg 
recovery. After the leg with a small temporal kinematic margin is lifted from the ground, the 
coordinator then accelerates the vehicle until its speed reaches the desired value. The coordinator 
also modifies directional commands by perturbing the vehicle trajectory in a lateral direction to 
try to provide a larger stability margin when necessary. 

The free gait coordinator is written in Symbolics Prolog and is listed in Figure 6. This pro- 
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gram is composed of three functional groups of predicates. The first group controls the flow of 
the whole program, while the second generates commands for the vehicle body and legs. The last 
group is responsible for bridging between the program written in Prolog and the program in Fla- 
vors. This is accomplished through the LISP function call facility provided by Symbolics Prolog. 
Specifically, anything following the ”is” predicate in a Prolog clause may be either a Prolog arith- 
metic function or the name of a LISP function [21]. If a LISP function name follows the ”is” 
predicate, it is evaluated according to its definition inside the LISP environment. In the program 
of Figure 6, all the names following the ”Is’’ predicates are names of LISP functions, and make 
connections to the LISP environment. A returned value resulting from a LISP function call may 
be used to instantiate a variable preceding the ”is” Prolog predicate or to test whether the 
returned value matches a value preceding the ”is** predicate. In the former case, the subgoal *^ls’* 
always succeeds, but In the latter case, only when two values agree does the *fis” subgoal succeed. 
For example. In Figure 6, the second clause, ’’initialize”, has an ”is” subgoal in its right side 
expression. The ”Is” predicate Is followed by a LISP function ’’inits”, and the ’’Inits” function is 
evaluated inside the LISP environment. A returned value from the result of the function call Is 
used to instantiate the dummy variable ”X” without further usage, and the subgoal ’’initialize” is 
simply realized when the ’’inits” LISP function internally initiates a series of processes. 
Specifically, when the ’’inits” function is called, all the internal component objects of the robot 
object, ”ASV”, are instantiated from the Flavor definitions to complete the robot object. In this 
process, an empty ”ASV”, which exists before the ’’Inits” LISP function is called, starts to fill its 
component slots with the body and six leg objects first, and then the body and the six legs fill 
their empty slots with sub-objects. This process continues until an object is reached which does 
not lack any sub-object necessary to fill its internal slots, such as the ’’Contact-Sensor” object in 
Figure 2. Thus, the ’’initialize” clause makes the robot object, ”ASV”, functionally complete to 
be used by the free gait coordinator. 
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The top level predicate of Figure 6, ”robot", belongs to the first group of clauses, and pro- 
vides the overall program control flow; i.e., it initializes the program and forms a loop for con- 
tinuous program execution when the "robot” goal is typed from a terminal. The program loop is 
formed both by the built-in predicate "repeat" which provides a way to generate multiple solu- 
tions through backtracking, that is, by making the goal "repeat" always succeed on backtracking, 
and by the built-in predicate, "fail", which initiates backtracking whenever it is tested. There- 
fore, the "loop" subgoal is executed again and again. The "loop" subgoal is responsible for 
another level of control flow of the program so that the Prolog program repeats a series of opera- 
tions, "get_command", "plan", and "execute"; i.e., it gets a desired velocity command from a 
human operator, plans vehicle motion based on the input command, and executes the planned 
vehicle motion. The "get_command" clause and the "execute" clause directly call their 
corresponding LISP functions, but the "plan" clause is refined into several subordinate Prolog 
clauses including "leg plan" and "body plan". The clauses or rules related to the plan predicate 
eventually communicate with the LISP environment to input states of the robot object, "ASV", 
and to output results from the planning process of the coordinator. Therefore, the data stored in 
the LISP robot object, "ASV", influences execution of the Prolog program, while the Prolog pro- 
gram modifys the data inside "ASV" using results from its execution. Thus, the whole plan por- 
tion of the free gait coordinator acts like a rule-based system being composed of three parts: infer- 
ence engine, rules, and fact beise. The plan clauses in Prolog and the status of the robot object, 
"ASV", in LISP, respectively, correspond to the rules and the fact betse. The Prolog default 
search strategy corresponds to the inference engine, which searches its rules using a depth-first 
method until it finds a proper rule to fire. Therefore, in the Prolog program, the positions of Pro- 
log clauses are interpreted as a priority preference among them. For example, the "leg_plan" 
portion consists of five "leg_plan" rules, and these rules are linearly ordered to express a free gait 
strategy that attempts to maximize the number of legs in the air. Thus, among the five rules. 
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the ”lift a leg” rule is written first. The second ”leg_plan” rule contains the second most favor- 
able way to use legs. When this rule succeeds, the number of legs in the air is not changed, but 
the vehicle improves its mobility by exchanging a supporting leg that limits its movement for a 
leg in the air. The next most favorable method is to do nothing as long as the vehicle maintains 
its stability. This idea is written in the third ”leg_plan” rule. The fourth way for ”leg_plan” to 
succeed is to place a leg on the ground to maintain stability, although the number of legs in the 
air is reduced by this action. The last ”leg_plan” rule represents the least favorable action since 
it both decreases the number of legs in the air and slows down the vehicle. 

The top level ”plan” clause makes the robot object, ”ASV”, follow a sequence of operations. 
First, it orders ”ASV” to update its internal states. On receiving the ”update_robot_state” com- 
mand, ”ASV” calculates the next body position based on the operator commands, and checks and 
collects internal state information for the coordinator, such as which legs are available to the 
coordinator. After completion of the updating process, the coordinator checks whether any leg is 
near its kinematic limit. If such a leg is found, the coordinator simply removes it from its sup- 
port pattern, and designates the leg as a limit leg so that the ”leg_plan” rules can provide a 
proper action to correct the leg limiting situation. In normal circumstances, the above mentioned 
leg is seldom found because, before a leg approaches its kinematic limit, the leg is lifted from the 
ground by the ”leg_plan” rules that always try to minimize the number of supporting legs. How- 
ever, sometimes this process is necessary when the vehicle directional command is abruptly 
changed or when the vehicle speed is too great. In the latter case, the legs on the ground quickly 
approach their temporal kinematic limit before the legs lifted from the ground are available to 
the coordinator. Therefore, the limit leg may be found either because of a planning error caused 
by an abrupt command change or because of temporal problems of lifted legs. After executing 
the ”check_tkm_limit” clauses, the coordinator tests the first "leg_plan” rule, ”lift_a_leg”. In 
the ”lift_a leg” rule, the coordinator checks the vehicle stability first. If the vehicle is stable, it 
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selects the leg with the smallest temporal kinematic margin among the supporting legs because 
such a leg will be the next to limit the vehicle mobility. Thus, the "lift_a_leg" rule asks ”ASV** 
whether there exists a leg with a smallest temporal kinematic margin by calling the LISP func- 
tion, "smallest_jtkm_leg”, and obtains the leg information through a returned value from the 
LISP function call. The leg information obtained is stored in the local variable, "A_leg", and is 
used to determine if "A_leg” can be removed from the support pattern without causing static ins- 
tability. If the vehicle is still stable without the leg, the coordinator causes this leg to be lifted 
from the ground by asserting a "lift” decision that will be used later by the "generatejdecision** 
predicate to send out the decision to the "ASV". Therefore, when this rule succeeds, the number 
of legs in the air is increased. The second most favorable rule is ’^exchange legs** which improves 
the mobility of the vehicle without affecting the number of legs in the air. This rule trys to 
exchange a leg, **LegA**, which has the smallest temporal kinematic margin among the supporting 
legs for a leg, **LegB**, which will provide the largest stability margin when the leg is added to the 
set of supporting legs. In this case, the stability margin associated with a leg is defined as the 
stability margin which results when the leg is placed on the terrain without changing legs in the 
support pattern except for excluding **LegA**. Because **LegB** is selected based on the stability 
criterion, if it is placed, then the stability of the vehicle tends to be maximized by this action. 
However, this maximization is somewhat limited because at most three legs can be compared to 
find **LegB**, and the designated foothold for each of these legs has been selected from footholds 
inside its working volume using the criterion of temporal kinematic margin. If **LegB** is found, 
then the **exchange_legs** rule does another test to determine whether the temporal kinematic 
margin of **LegB** is larger than that of **LegA** because, if **LegB** has a smaller temporal 
kinematic margin than that of **LegA**, replacing **LegA** with **LegB** would cause the kinematic 
problem to become more serious rather than improved. If the above test succeeds, then a leg 
exchange is performed. Consequently, the mobility of the vehicle is improved without changing 
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the number of legs in the air. 

The third most favorable ’4eg_plan^’ is doing nothing eis long as the vehicle is stable, because 
without changing the number of legs in the air the vehicle can still move while following the 
desired vehicle commands. However, when this rule succeeds, the vehicle mobility is not 
improved. The fourth rule can succeed only if the stability test of the preceding rule falls. Since 
the planner runs one second in advance, this means that the vehicle is about to become unstable. 
In this case, the coordinator selects one leg from the available legs and causes it to support the 
body to restore stability. Though this action decreases the number of legs in the air, it is neces- 
sary to maintain the vehicle stability. When selecting such a leg, the coordinator tries to choose 
the leg which will give the largest stability margin among the available legs. Again, the vehicle 
stability is maximized in a limited sense. However, this attempt may or may not be successful, 
either because no leg available to the coordinator generates a sufficient stability margin if it were 
to be used to support the body, or because no legs are yet available to the coordinator. If a proper 
leg is found, it is commanded to support the body and the number of legs in the air Is decreased. 
Therefore, this rule Is least favorable to the coordinator among the above four ’’leg_plan’’ rules, 
but is necessary to maintain the vehicle stability. 

The last ”leg_plan” rule is ’^wait for legs’\ which both sacrifices the mobility of the vehicle 
and decreases the number of legs in the air. The first sub-rule is ’Hry_new_foothold” which con- 
tinuously cisslgns new footholds to the legs available to the coordinator until the coordinator finds 
a leg able to make the vehicle stable. This process tends to reduce the future mobility of the 
vehicle because the newly assigned foothold has a smaller temporal kinematic margin than that of 
the initially assigned foothold. To minimize the seriousness of this effect, new footholds are 
assigned In the order of their temporal kinematic margin. Therefore, when a foothold is found 
which can make the vehicle stable, it may have a larger temporal kinematic margin than the 
smallest temporal kinematic margin that the leg can provide, but it has a smaller temporal 
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kinematic margin than that provided by the initially assigned foothold. 

The next sub-rule performs the "recovery” action. The central idea of this action is to help 
the "place a leg" and "try_new_foothold" rules so that they can find a suitable leg in the next 
control cycle. Because the leg placement rules handle one leg during each control cycle, some- 
times the rules cannot find a suitable leg to solve a mobility problem of the vehicle, but by using 
two or three legs the problem could be solved. Instead of increasing the complexity of the 
"leg plan" rules so that they can handle two or three legs in one control cycle, the problem is 
solved through multiple control cycles by Introducing the "recovery" rule, while concurrently the 
speed of the vehicle is reduced in order to preserve its current stability. While simple, this 
approach is not guaranteed to find an ideal solution that could be obtained from generalized rules 
capable of handling multiple leg placements. When the "recovery" rule is tried, no single leg is 
able to make the vehicle stable using the previous rules. In this case, all the legs available to the 
coordinator are identically useless. Thus, any one leg can be arbitrarily chosen and made to sup- 
port the vehicle body. This action causes the vehicle to have a new support pattern, and 
increases the chance that the coordinator can find an additional leg that makes the vehicle stable 
in the next control cycle. If such a leg is found in the next cycle, a two leg placement solution is 
found through two consecutive cycles the "leg plan" rules. If an adequate leg is not found again, 
then the above process will be repeated once more with the result that all the available legs are 
consumed by the "recovery" rule, since at most three legs can be in the air at any given time. 

The last sub-rule of "wait_for_legs" simply slows down the vehicle, and if a limit leg is found 
in the "check__tkm__limit" rules, then this rule restores the leg to the support pattern because the 
previous "leg_plan" rules could not make the vehicle stable without the limit leg. Moreover, the 
temporal kinematic margin of this leg will be increased because the vehicle speed is reduced by 
this rule. Specifically, if the limit leg information is asserted by the "check__tkm_limit" rules, this 
information is retracted from the Prolog data base by this rule so that the limit leg is not lifted 
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from the ground. 

The next step in the "plan” rule is "body_plan" which is composed of two sub-goals. The 
first one is "speed^plan" which checks whether a vehicle speed reduction is requested from the 
"leg_plan" rules. If requested, it retracts the request and makes the vehicle slow down. If not, it 
makes the vehicle speed up to follow a desired speed command. The second sub-goal, 
"trajectory_plan", helps to increase the stability margin of the vehicle. Because the stability 
margin is omni- directional, there exists one shortest line segment between the center of the body 
and the boundary of the support pattern. The main idea of the "trajectory-plan" predicate is to 
increase the length of this line segment. The method adopted here is to push the center of the 
body away from the boundary of the supporting pattern in the direction parallel to the shortest 
line segment until the stability margin is larger than a specified desired value used for the stabil- 
ity test in the "leg^plan" rules. In order to implement the above idea, a "recovery vector" is 
defined such that it points away from the boundary of the supporting pattern along the line seg- 
ment that determines the stability margin at that moment. The magnitude of the vector is pro- 
portional to the reciprocal of the stability margin. The recovery vector is internally interpreted 
2LS a recovery velocity, and is superimposed on the operator velocity command so that the vehicle 
is pushed away from the boundary. More precisely, only the component of the recovery vector 
perpendicular to the velocity command is used in order to eliminate a speed disturbance in the 
direction commanded by an operator during such a push operation. 

After planning leg and body motions, the coordinator sends decisions to the robot object, 
"ASV", one by one, until all the decisions in the Prolog data base are exhausted. Specifically, two 
activities, a decision retraction from the Prolog data bcise and the decision dispatch to "ASV", 
are repeated by the "fail" predicate at the end of the "generate_decision" clause until all the deci- 
sions are popped from the Prolog data base. When all the decisions are thus removed from the 
data base, the second "generate decision" clause is tested to determine whether or not a limit leg 
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exists in the data base. If this is true, then the second clause deletes the limit leg from the data 
base and sends a lift decision to the robot object, ”ASV’’. 

In the execution process, the coordinator instructs ”ASV” to execute the decisions given to it 
in the planning process. The six leg plan machines, the six leg control machines, the six leg exe- 
cutors, and the six contact sensors start functioning according to the hierarchy shown in Figure 2. 
The decisions from the coordinator cause state transitions of the six leg plan machines, and the 
state machines send commands to their subordinate control machines so that the control 
machines can update their control states. The control machines instruct their leg executors to 
move their legs depending on the control states and monitor states of their contact sensors to 
check leg touch-down events. Finally, the coordinator calls the ”graphical_dlsplay” LISP func- 
tion to draw an updated robot image on the screen of the simulation computer. Thereby one con- 
trol cycle is completed. This action continues until the operator interrupts the program. 

5. Discussion 

The gait selection problem for a walking machine is inherently an ill-defined problem [28] 
because neither the goal state, nor the rules (or operators) available for reducing the state 
difference to reach the goal state are clearly specified. Instead, two general minimality criteria 
are known: stability margin and temporal kinematic margin. Thus, it is necessary to add more 
structure to this problem using constraints in order to make it more manageable. A frequently 
used constraint is to make a walking machine use a periodic leg stepping sequence, follow a 
straight-line trajectory, and walk on flat level terrain free from prohibited stepping areas. Using 
this constraint, a family of well understood optimal gaits, called wave gaits is found [7, 9j. How- 
ever, for more general applications of walking machines, such as locomotion on rough terrain 
along an arbitrary trajectory, this constraint is too restrictive. Rather, it should be either relaxed 
or eliminated. If the constraint is removed, free gaits result. Thus, free gaits include all possible 
periodic gaits. In the specific free gait implementation of this study, instead of limiting the 



20 



stepping sequence and body trajectory, an overall strategy is added to structure the gait problem; 
namely, to minimize the number of supporting legs to the greatest extent possible. 

Differing from a tripod gait that exists as a closed-form solution, free gaits are generated by 
on-line optimization depending on the situation currently confronted by a walking machine. 
Therefore, the first performance measure of any free gait coordination algorithm is whether it 
reduces to a tripod gait in situations where this gait is known to be optimal. From the computer 
simulation, it is observed that the gait generated by the coordination algorithm when it drives 
the ASV along a straight line on a flat level terrain containing no obstacles is essentially a tripod 
gait. Moreover, when a small number of obstacles are added to the terrain, the program still uses 
tripod-like gaits. However, as the percentage of forbidden cells on the terrain is increased, as 
would be expected, the gaits generated by the program change. 

Perhaps the most distinctive departure of leg stepping from tripod gaits is observed when the 
ASV makes a turn-in-place motion. Ideally, during this motion, the middle legs should show less 
frequent leg movements compared with those of the front and the rear legs. Evidently, tripod 
gaits cannot exhibit such leg motions since the middle legs are cycled at the same rate as the 
front and rear legs. The free gaits generated by the program clearly show the expected behavior. 
Specifically, during a turning-in-place motion of 90 degrees or more, the middle legs are not usu- 
ally changed during several motion cycles of the front and back legs. 

To obtain an objective performance evaluation, the model vehicle was made to walk over ter- 
rain containing randomly generated obstacles while following a standard trajectory. Along this 
trajectory, three types of motion are sequentially performed. First of all, after the vehicle is ini- 
tialized, it moves in the forward direction. It then makes a 90 degree turn-in-place at the middle 
of the terrain in the counter-clockwise direction, and finally performs side stepping until it 
reaches the edge of the model terrain [ll]. For these three trajectory segments, the vehicle is 
sequentially commanded with three different velocity inputs: a one foot per second velocity input 
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in the forward direction, a one-twentieth radian per second turning velocity input, and a half foot 
per second side-stepping velocity input in the right side direction. A typical terrain example is 
shown in Figure 1. In this example, eighty percent of the area is classified as permitted cells. 

Performance tests were based on trials on 10 different terrains with the same probability of 
occurrence of permitted cells. The general tendency of the results obtained in this evaluation is 
as would be expected; namely, the larger the probability of permitted cells, the better the chance 
that a vehicle can cross the randomly generated terrain. No failures to complete the standard 
trajectory were observed for any terrain in which permitted cells occupied 60 percent or more of 
the area of the whole simulation terrain. For terrain with 50 percent, 40 percent, and 30 percent 
permitted cells, the tests showed 8, 9, and 8 successes out of ten trials respectively. However, the 
success rate was sharply reduced for terrain containing less than 20 percent permitted cells, and 
no successful motion was observed when 10 percent or less of the total terrain cells was permit- 
ted. Therefore, practically, when more than 30 percent of the total terrain cells are permitted, 
the program makes the ASV maneuver without great difficulty. The average time spent to run 
one test with a Symbolics 3675 Lisp Machine is about 15 minutes, almost 10 times slower than 
the simulation time on which all the internal calculations are based. 

One of the advantages of rule-based control of stepping is the ease of modification of stepping 
logic resulting from the fact that each rule defines a small, relatively independent piece of 
behavior. To demonstrate this advantage, two alternative rule sets were compared to the origi- 
nal rule set. In the first experiment, the ”exchange_legs” rule was removed from the ”leg_plan” 
rules because this rule was suspected of being redundant. When this is done, the ”lift_a_leg^* rule 
and the ”place_a_leg” rule implicitly share leg exchange responsibility. However, it was found 
that the performance of the new rule configuration is poorer than that of the complete rules. 
More frequent planning failure is observed on randomly generated complicated terrain. Gaits 
generated by the new rules set are not so close to tripod gaits as those generated by the complete 
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rules, although during a turn-in-place motion the new rule set also clearly treats the middle legs 
differently from the other four legs. Therefore, it can be concluded that the **exchange_legs^’ rule 
improves performance. This rule provides a shortcut of the logical process and works somewhat 
like compiled knowledge in a human or animal brain obtained from experience. 

If the ^*lift_a_leg** rule is also removed from the ”leg_plan" rules, the coordinator instructs 
the ASV to use as many legs as possible to support its body. Thus, the resulting leg stepping 
strategy has almost an opposite meaning compared to the original one. As would be expected, 
the gaits generated by the rules without these two leg plan rules have little resemblance to tripod 
gaits, even on flat level terrain. The overall performance of the simplest rule set on rough terrain 
is much better than that of the former simplified rule set, but slightly inferior to that of the origi- 
nal rules. It frequently slows down the vehicle, but the terrain adaptability to rough terrain is 
almost the same as that of the original rule set. 

The overall conclusion from simulator tests to date is that the original rule set is a good one. 
Specifically, when the ASV is presented with with easy terrain and essentially forward motion, a 
tripod gait is produced by the free gait coordinator. On the other hand, for turning in place or 
moving over terrain with few available footholds, the free gait algorithm seems to make "intelli- 
gent” decisions and displays a remarkable ability to pick its way through regions with sparse 
footholds. Attempts to simplify the rule set presented have so far resulted in deterioration of 
their behavior. 



23 



6, Summary and Recommendations 



The main purpose of this study was to demonstrate the value of a multiple paradigm pro- 
gramming environment in the development of software for coordination of motion for the ASV 
walking machine. An important secondary goal was using such a facility to better explain the 
algorithm developed in [ll], and to investigate the effect of changes in its basic strategy. With 
respect to the first objective, the authors believe that the code contained in this report is much 
easier to read and to modify than the version presented in [ll] and [15], which used, respectively, 
functional programming (Common Lisp), and imperative programming (Pascal). With regard to 
the second objective, we find Prolog to be more powerful than English in explaining the logical 
conditions for state transitions in the leg plan finite state machines used for implementing the 
coordination algorithm. It is especially important that the rules of Figure 6 can be read declara- 
tively by humans and procedurally by a computer. One of the consequences of this fact is that 
modification of stepping algorithms is remarkably easier in Prolog than in Lisp or Pascal. 

It is our belief that our simulation studies to date have shown that the full free gait algo- 
rithm developed in this report is appropriate for physical experiments with the ASV. We believe 
that the multilingual approach we have used should be retained in such experiments, and that 
future vehicle computer upgrades should provide a platform for efficient execution of Prolog and 
extended Common Lisp. Finally, we recommend that program development continue to be done 
on a Lisp machine with facilities for downloading to a runtime system in the vehicle computer. 

Among studies remaining to be conducted are inclusion of vehicle inertia in the simulation, 
eflfects of leg motion on the location of the vehicle center of gravity, and a better simulation of 
the vision system. Also, in the experiments reported herein, a constant small value for the 
minimum vehicle stability and for the minimum temporal kinematic margin was used. It is 
important to determine how variation in these threshold values affects safety and maneuverabil- 
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ity, especially when vehicle inertia is taken into account. Such a study will be undertaken early 
in the next phase of this research along with an investigation of further changes to the Prolog rule 
set used for the top level of motion coordination. 
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Figure 1: Typical Simulation Terrain and Vehicle 
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Figure 2: Hierarchy of simulation objects 
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Figure 3. Leg motion relative to body during 

forward body motion over level terrain 
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T1: 0.6 Seconds 
T2: 1.0 Second 
T3: 0.4 Seconds 
T4: 0.6 Seconds 



Figure 4: State diagram for Leg Control Machine 



32 




Figure 5: State diagram for Leg Plan Machine 
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%%% Mode: PROLOG; Package: robot-rules; Default-character-style: (:FIX 

RGE); -*- 

robot initialize, repeat, loop, fail. 

initialize X is inits . 

loop :- get_cominand, plan, execute, !. 

get_command :- X is read_joystick . 

plan update_robot_state , check_tkin_liinit , 

leg_plan, body_plan, generate_decision, ! . 

update_robot_state :- X is update_robot_status . 

check_tkm_liinit :- A_leg is at_tkm_limit , A_leg \== nil, 

asserta (limit_leg (A_leg, lift) ) . 

check_tkm_liinit . 

leg_plan :- lift_a_leg, 
leg_plan : - exchange^legs . 
leg_plan stable. 

leg_plan :- place_a_leg. 
leg_plan wait_f or_legs . 

stable :- Condition is stable_p. Condition == t. 

lift_a_leg :- stable, A_leg is smallest_tkin_leg , A_leg \== nil. 

Condition is stable_without (A_leg ) , Condition == t, 
asserta (decision (A_leg,_, lift) ) . 

exchange^legs :- stable, LegA is sniallest_tkin_leg, LegA \== nil, 

LegB is max_sin_leg (LegA) , LegB \== nil, 

Condition is has_more_tkin (LegB, LegA) , 

Condition == t, 

asserta (decision (LegA, LegB, exchange ) ) . 

place_a_leg A_leg is max_sm_leg (_) , A_leg \== nil, 
asserta (decision (A_leg, _, place) ) . 

wait_f or_legs :- try_new_f oothold . 

wait_f or_legs :- recovery, asserta (reduce_speed) . 

wait_f or_legs :- asserta (reduce_speed) , restore_limit_leg . 

try_new_f oothold A_leg is leg_with_new_f oothold, A_leg \== nil, 

asserta (decision (A_leg, _, place) ) . 

recovery :- A_leg is do_recovery, A_leg \== nil, 

asserta (decision (A_leg, _, place ) ) , restore_limit_leg . 

restore_limit_leg ;- retract (limit_leg (A_leg, lift) ) . 
restore_liinit_leg . 



Figure 6; Free Gait Coordinator 



: ROMAN iV 
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:lDody_plan speed__plan, tra jectory_plan . 

3 peed_plan retract ( reduce_speed) , slow_down. 

3peed_plan speed_up. 

speed_up X is speed_up_robot . 

slow_down X is slow_down_robot . 

tra jectory^plan stable_m, restore_tra jectory . 

tra jectory_plan modify_tra jectory . 

stable_m Condition is stable__p_m, Condition == t. 
restore^tra jectory X is restore_conunand . 
modi fy_tr a jectory X is modif y_command . 

generate_decision retract (decision (A_leg, B_leg, A_decision) ) , 

X is send_decision (A_leg, B_leg, A_decision ) , 
generate_decision retract (limit^leg (A_leg, A_decis ion) ) , 

X is send_decision (A_leg, A_decision) , fail 

generate_decision . 



execute execute_motion, draw_robot, !. 
execute_motion X is execute_planned_motion . 
draw_robot X is graphical_display . 



Figure 6: continued... 



fail. 
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Lisp Code for ASV Simulation 



dy-controller . lisp Wed Mar 30 15:27:41 1988 1 

; Mode; LISP; Package: BODY; Syntax: Common-lisp 

body-controller definition 

.ef flavor body-controller (joystick-command-regulator terrain-regulator 

H-calculator 

body-trans-ratel body-rot ate-ratel 
body-t rans-rate6 body- rot at e- rate 6 
body-t rans-ratelO body-rot at e-rat el 0 
HI inv-Hl H6 inv-H6 HIO inv-HlO 
H inv-H body-t body-r) 

0 

; initable-instance-variables) 



lefmethod (init body-controller) 

0 

(setf joystick-command-regulator (make-instance ' joystick-command-regulator) ) 
(setf terrain-regulator (make-instance 'terrain-regulator)) 

(setf H-calculator (make-instance 'H-calculator)) 

(init joystick-command-regulator) 

(init terrain-regulator) 

(setf H (init H-calculator) ) 

(init-body-rates self) 

(init-H self) 

HI 

) 



lefmethod (init-body-rates body-controller) 

0 

(setf body-trans-ratel ' (0 0 0) ) 

(setf body-trans-rate6 '(0 0 0)) 

(setf body-trans-ratelO ' (0 0 0) ) 

(setf body-rotate-ratel ' (0 0 0) ) 

(setf body-rotate-rat e6 ' (0 0 0) ) 

(setf body-rotate-ratelO ' (0 0 0) ) 

) 



lefmethod (init-H body-controller) 

0 

library fucntion : ident 
(setf HI H) 

(setf H6 H) 

(setf HIO H) 

(setf inv-H (matrixinv H) ) 

(setf inv-Hl inv-H) 

(setf inv-H6 inv-H) 

(setf inv-HIO inv-H) ) 
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(defmethod (control body-controller) 

(joystick-command deceleration-factor estimated-support-plane) 

(setf H HI) 

(update self joystick-command deceleration-factor estimated-support-plane) 
(save self) 

(dotimes (i 10) 

(cond ( (equal i 0) 

(setf body-trans-ratel body-t) 

(setf body-rotate-ratel body-r) 

(setf HI H) 

(setf inv-Hl inv-H) ) 

( (equal i 5) 

(setf body-t rans-rate6 body-t) 

(setf body-rotate-rate6 body-r) 

(setf H6 H) 

(setf inv-H6 inv-H) ) 

( (equal i 9) 

(setf body-trans-ratelO body-t) 

(setf body-rotate-ratelO body-r) 

(setf HIO H) 

(setf inv-HlO inv-H) ) ) 

(update self joy stick- command deceleration-factor estimated-support-plane) 

) 

(restore self) ) 



(defmethod (update body-controller) 

(joystick-command deceleration-factor estimated-support-plane) 
; internally used by control method 

(let* ( (t-command (regulate terrain-regulator 

estimated-support-plane H) ) 

(j-command (regulate joystick-command-regulator 

joystick-command deceleration-factor) ) 

) 

(setf body-t (list (first j-command) (second j-command) 

(third t-command) ) ) 

(setf body-r (list (first t-command) (second t-command) 

(third j-command) ) ) 

(setf H (new-H H-calculator body-t body-r) ) 

(setf inv-H (matrixinv H) ) ) ) 



(defmethod (restore body-controller) 

0 

; internally used by control method 
(restore joyst ick-command- regulator ) 
(restore terrain-regulator) 

(restore H-calculator) ) 



(defmethod (save body-controller) 

0 

; internally used by control method 
(save joystick-command-regulator) 
(save terrain-regulator) 

(save H-calculator) ) 
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efmethod (get-body-t rans-ratel body-controller) 

0 

body-t rans-ratel) 



efmethod (get-body-rotate-ratel body-controller) 

0 

body-r ot ate- rate 1) 



efmethod (get-body-trans-ratelO body-controller) 

0 

body-trans-ratelO) 



.efmethod (get-body-rotate-ratelO body-controller) 

0 

body-rotate-ratelO) 



iefmethod (get-Hl body-controller) 



0 



HI) 



iefmethod (get-inv-Hl body-controller) 



0 



inv-Hl ) 



iefmethod (get-H6 body-controller) 

0 

H6) 



iefmethod (get-inv-H6 body-controller) 

0 

inv-H6) 



iefmethod (get-HlO body-controller) 

0 

HIO) 



iefmethod (get-inv-HlO body-controller) 

0 



inv-HlO) 
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. Mode: LISP; Syntax: Common-lisp; Package: BODY 



body flavor definition 






?f flavor body (stability-calculator support-plane-estimator 

body-controller owner 
estimated- support -plane 
decele rat ion- factor 
support -plane -clock 
modify-vector 
modify-vector-p 
stop-mot ion- flag 
joy-command) 

0 

: initable-instance-variables) 



^fmethod (slow-down body) 

0 

(setf deceleration-factor (t deceleration-factor 1) ) 
(if (> deceleration-factor 20) 

(setf deceleration-factor 20) ) ) 

sfmethod (speed-up body) 

0 

(setf deceleration-factor (- deceleration-factor 1) ) 
(if (< deceleration-factor 0) 

(setf deceleration-factor 0) ) ) 



ifmethod (stable-m body) 

(supporting- legs) 

(stable-m stability-calculator 

supporting-legs (get-HlO body-controller) ) ) 



ifmethod (stable-p-m body) 

(supporting-p-legs a-leg) 

(stable-p-m stability-calculator 

supporting-p-legs 
(get-Hl body-controller) ) ) 



sfmethod (stop-p body) 

0 

(let ( (trans-rate (get-body-t rans-ratel self) ) ) 
(equal (list (first trans-rate) 

(second trans-rate) ) 

' ( 0.0 0 . 0 ) ) ) ) 



5fmethod (modif y-command body) 

0 

(setf modify-vector 

(get-recovery-vector stability-calculator) ) ) 
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(defmethod (modify-command-p body) 

0 

(setf modif y-vector-p 

(get-recovery-vector-p stability-calculator) ) ) 



(defmethod (restore-command body) 

0 

(setf modify-vector ' (0 0 0) ) ) 



(defmethod (restore-command-p body) 

0 

(setf modify-vector-p '(0 0 0))) 



(defmethod (stop-motion body) 
(a-leg) 

(setf stop-motion-flag a-leg)) 



(defmethod (restore-motion body) 

0 

(setf stop-motion-flag nil) ) 



(defmethod (init body) 

0 

(setf deceleration-factor 0) 

(setf modify-vector-p ' (0 0 0) ) 

(setf modify-vector ' (0 0 0) ) 

(setf stop-motion-flag nil) 

(setf support-plane-clock 10) 

(setf stability-calculator 

(make-instance ' stability-calculator) ) 

(setf support-plane-estimator 

(make-instance 'support-plane-estimator) ) 
(setf body-controller 

(make-instance 'body-controller) ) 

(init stability-calculator) 

(init support-plane-estimator) 

(init body-controller) 

) 

(defmethod (get-modif y-vector body) 

0 

(vectsub modify-vector 

(dotprod modify-vector 

(normalize-vector joy-command) ) ) ) 



(defmethod (get-modify-vector-p body) 

0 

modify-vector-p) 
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lefmethod (calculate-motion body) 

' (joystick-command legs) 

(setf joy-command joystick-command) 

(cond ( (equal support-plane-clock 10) 

(setf estimated-support-plane 

(get-plane support-plane-estimator legs) ) 

(setf support-plane-clock 0) ) ) 

(setf support -plane-clock (+ support-plane-clock 1) ) 

(cond 

I ( (or stop-motion-flag (null modif y-vector-p) ) 

! (control body-controller 

I '(0 0 0 ) 

0 estimated-support-plane) ) 

(modif y-vector-p 
(control body-controller 

(vectadd joy-command (get-modify-vector-p self) ) 
deceleration-factor estimated-support-plane) ) 

(t 

' (control body-controller 

I (vectadd joy-command (get-modify-vector self) ) 

I deceleration-factor estimated-support -plane) ) ) ) 



f efmethod (get-estimated-support-plane body) 

0 

'estimated- support -plane) 



lefmethod (get-body-trans-ratel body) 

O 

(get-body-trans-ratel body-controller) ) 



^lefmethod (get-body-rotate-ratel body) 

0 

(get-body-rotate-ratel body-controller) ) 



lefmethod (get-body-trans-ratelO body) 

0 

(get-body-trans-ratelO body-controller) ) 



lefmethod (get-body-rotate-ratel 0 body) 

0 

(get -body-rot ate- rat el 0 body-controller) ) 



lefmethod (get-Hl body) 

0 

(get-Hl body-controller) ) 



lefmethod (get-inv-Hl body) 

0 

(get-inv-Hl body-controller) ) 
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(defmethod (get-H6 body) 

0 

(get-H6 body-controller) ) 



(defmethod (get-inv-H6 body) 

0 

(get-inv-H6 body-controller) ) 



(defmethod (get-inv-HlO body) 

0 

(get-inv-HIO body-controller) ) 



(defmethod (more-stable body) 

(supporting-legs legl leg2) 

(more -St able stability-calculator 

supporting-legs (get-HlO body-controller) 
legl leg2) ) 



(defmethod (stable body) 

(support ing- legs) 

(stable stability-calculator 

supporting-legs (get-HiO body-controller) ) ) 



(defmethod (stable-p body) 

( support ing-p-legs) 

(stable-p stability-calculator 

supporting-p-legs (get-Hl body-controller) ) ) 
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ymnand-regulator . lisp . 

.1 . Mode: LISP; Syntax: Common- lisp; Package: BODY 

.| ********************************************************* 



! regulator flavor definition 

;i 



Jef flavor regulator ( (max-a 3.2174) (time-const 0.5) (sampling-t ime 0.1)) 

0 

' : initable-instance-variables) 



lefmethod (filter regulator) 

(desired-x present-x) 
first order regulation 

(let ( (del-vel (/ (- desired-x present-x) time-const) ) ) 

(+ (* (g-limitor self del-vel) sampling-time) 
present-x) ) ) 



jlefmethod (g-limitor regulator) 

(del-vel) 

.limit acceleration to 3.2174 ft/(sec*sec) or 0.1 G. 
! (cond ( (> del-vel max-a) max-a) 

( (< del-vel (- max-a) ) (- max-a) ) 

(T del-vel) ) ) 






joystick-command-regulator flavor definition 



lefflavor joystick-command-regulator (body-trans-rate-x 

body-trans-rate-y 
body-rotate-rate- z 
old-body-trans-rate-x 
old-body-trans-rate-y 
old-body-rotate-rate-z) 

(regulator) 

: initable-instance-variables) 



iefmethod (init joystick-command-regulator) 

0 

(setf body-trans-rate-x 0.0) 

(setf body-trans-rate-y 0.0) 

(setf body-rotate-rate-z 0.0) 

(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 
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(defmethod (regulate joystick-coiranand-regulator) 

( joystick-command deceleration-factor) 

(if (<= deceleration-factor 0) 

(setf deceleration-factor 0.5)) ; remove effect of deceleration-factor, 

(let* ( (d-const 0.5) 

(x (* (first joystick-command) (/ d-const deceleration-factor) ) ) 

(y (* (second joystick-command) (/ d-const deceleration-factor) ) ) 

(r (* (third joystick-command) (/ d-const deceleration-factor) ) ) ) 

(setf body-trans-rate-x (filter self x body-trans-rate-x) ) 

(setf body-trans-rate-y (filter self y body-trans-rate-y) ) 

(setf body-rotate-rate-z (filter self r body-rotate-rate-z) ) ) 

(if (< (abs body-trans-rate-x) 0.02) (setf body-trans-rate-x 0.0)) 

(if (< (abs body-trans-rate-y) 0.02) (setf body-trans-rate-y 0.0)) 

(if (< (abs body-rotate-rate-z) 0.005) (setf body-rotate-rate-z 0.0)) 
(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 



(defmethod (restore joystick-command-regulator) 

0 

(setf body-trans-rate-x old-body-trans-rate-x) 

(setf body-trans-rate-y old-body-trans-rate-y) 

(setf body-rotate-rate-z old-body-rotate-rate-z) 

(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 



(defmethod (save joystick-command-regulator) 

0 

(setf old-body-trans-rate-x body-trans-rate-x) 

(setf old-body-trans-rate-y body-trans-rate-y) 

(setf old-body-rotate-rate-z body-rotate-rate-z) 

(list body-trans-rate-x body-trans-rate-y body-rotate-rate-z) ) 



tit rol-ma chine . lisp Wed Mar 30 15:27:45 1988 1 

. Mode: LISP; Package: LEG; Syntax: Common-lisp; Base; 10 

★★★★★★★★★★★★★★★★★★★★A**************************************** 

state flavor definition 

★★★★★★★★★★★★★A*********************************************** 

efflavor state (name next-state) 

0 

: initable-instance-variables) 



efmethod (state-name state) 

0 

name) 



efmethod (set-next-state state) 
(a-state) 

(setf next-state a-state) ) 






sync-state flavor definition 



efflavor sync-state ( (time 0) (del-t 0.1) time-out) 
(state) 

: initable-instance-variables) 



efmethod (change sync-state) 

0 

(setf time (+ time del-t)) 
(cond ( (>= time time-out) 
(setf time 0) 
next-state) 

(t self))) 



efmethod (get-time sync-state) 

0 

time) 
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control -machine . lisp 



; async-state flavor definition 

"***★*★***★*★★*★★*★**************************************** 

r 



(defflavor async-state ( (conutiand nil) (observation nil)) 
(state) 

: initable-instance-variables) 



(defmethod (change async-state) 

(given- command observed-event) 

(cond ( (and (not observation) 

(equal given-command command) ) 
next-state) 

( (and (not command) 

(equal observed-event observation) ) 
next-state) 

(t self))) 



state-machine flavor definition 

(defflavor state-machine (state owner) 

0 

: initable-instance-variables) 



(defmethod (state-name state-machine) 

0 

(state-name state) ) 



control-state-machine flavor definition 



(defflavor control-state-machine ( (command nil) (observation nil) 

contact-sensor executor) 



(state -machine) 

: initable-instance-variables) 



(defmethod (init control-state-machine) 
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(leg-name) 

(if (member leg-name ' (legl leg4 leg5) ) 
(init-control-machine self 'support) 
(init-cont rol-machine self 'ready)) 

(setf contact-sensor (leg-contact-sensor owner) ) 
(setf executor (leg-executor owner) ) ) 



lefmethod (init-control-machine control-state-machine) 

(a- state -name) 

internally used by init method 

(let (return lift support contact descent advance ready) 



(setf 


return 








(make -instance 


' sync-state 








:name 'return : time-out 


0.6)) 


(setf 


lift 

(make- instance 


' sync-state 








inamie 'lift : time-out 0. 
: next-state return)) 


4 


(setf 


support 
(make -instance 


' async-state 
: name 'support : command 
:next-state lift)) 


' recover-command 


(setf 


contact 








(make- instance 


' sync-state 

: name 'contact : time-out 
: next-state support) ) 


1.0 


(setf 


descent 
(make -instance 


' async-state 








: name 'descent : observation 'contact-confirm 






:next-state contact)) 




(setf 


advance 
(make -instance 


' sync-state 

:name 'advance : time-out 
: next-state descent) ) 


0.6 


(setf 


ready 

(make -instance 


' async-state 





: nsime 'ready : command 'deploy-command 
: next-state advance)) 



(set-next-state return ready) 



(setf state (cond 



) 

) 



( (equal a-state-name 
ready) 

( (equal a-state-name 
advance) 

( (equal a-state-name 
descent) 

( (equal a-state-naime 
contact) 

( (equal a-state-name 
support) 

( (equal a-state-name 
lift) 

( (equal a-state-name 
return) ) ) 



(state-name 


ready) ) 


(state-name 


advance) ) 


(state-name 


descent) ) 


(state-name 


contact) ) 


(state-name 


support) ) 


(state-naune 


lift)) 


(state-name 


return) ) 



lefmethod (change control-state-machine : before) 
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0 

(cond ( (typep state ' async-state) 

(if (sensing contact-sensor) 

(setf observation ' contact -con firm) 
(setf observation nil) ) 

) ) ) 



(defmethod (change control-state-machine) 

0 

(cond ((typep state 'sync-state) 

(setf state (change state) ) ) 

(t (setf state (change state command observation) ) ) ) ) 



(defmethod (change control-state-machine : after) 

0 

; send command to executor with sync-state-time 
(send-command executor (state-name state) ) 

(if (typep state 'sync-state) 

(set-time executor (get-time state) ) 
(set-time executor nil) ) ) 



(defmethod (send-command control-state-machine) 
(a-command) 

(setf command a-command) ) 
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; mode: lisp; syntax : common- lisp; Package: TERRAIN 

********************************************************* 

display . globals 

★★★★★★A************************************************** 

efvar eye-space nil) 
efvar middle-of-screen nil) 



efvar terrain- joystick) 
efvar graph-terrain) 
efvar graph-asv) 

efvar terrain-sample nil) 
efvar terrain-type "Random”) 
efvar terrain-angle nil) 
efvar terrain-percent 0) 
efvar terrain-seed 123) 



display . library 



efun draw-to-earth (a-point) 

(let ( (draw-pt (make-displayable 
middle-of-screen 

(transform eye-space a-point) ) ) ) 
(zl-user : draw-to 

(list (truncate (first draw-pt) ) 
(truncate (second draw-pt) ) ) 
zl-user : * robot-window*) ) ) 



efun draw-to-earth-d (a-point) 

(let ( (draw-pt (make-displayable 
middle-of-screen 

(transform eye-space a-point) ) ) ) 
( zl-user : draw-to-d 

(list (truncate (first draw-pt) ) 
(truncate (second draw-pt) ) ) 
zl-user : *robot -window*) ) ) 



efun erase-to-earth (a-point) 

(let ( (draw-pt (make-displayable 
middle-of-screen 
(transform eye-space a-point)))) 
(zl-user : erase -to 

(list (truncate (first draw-pt) ) 
(truncate (second draw-pt))) 
zl-user : *robot-window* ) ) ) 
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(defun eye-trans (eye-pt) 

; eye-pt (radius alpha beta) 

; eye := orient *trans (0^ 0, -r) *rot (x, -beta) *rot (y, -alpha) *trans (-x , -y , -z) 

; returns eye-space 

; library : ident, t ran smat , rotate, mat rixmult 
(let* ((orient (ident) ) 

(rot nil) (trans nil) (eye nil) 

(radius (first eye-pt)) (alpha (second eye-pt)) (beta (third eye-pt)) 
(center-of-interest (list (/ (terrain-max-x graph-terrain) 2) 

(/ (terrain-max-y graph-terrain) 2) 0) ) ) 

(setf (aref orient 2 2) -1.0) 

(setf trans (transmat 0 0 (- radius) ) ) 

(setf eye (matrixmult orient trans) ) 

(setf rot (rotatemat 'y-axis (- alpha))) 

(setf eye (matrixmult eye rot) ) 

(setf rot (rotatemat 'x-axis (- beta))) 

(setf eye (matrixmult eye rot) ) 

(setf trans (transmat (- (first center-of-interest) ) 

(- (second center-of-interest)) 

(- (third center-of-interest) ) ) ) 

(matrixmult eye trans) ) ) 



(defun make-displayable (middle pt) 

(let ((scale 5000.0) 

(x (first pt)) (y (second pt) ) (z (third pt))) 
(list (+ (* scale (/ x z) ) (first middle)) 

(+ (* scale (/ y z) ) (second middle) ) ) ) ) 



(defun move-to-earth (a-point) 

(let ( (draw-pt (make-displayable 
middle-of- screen 

(transform eye-space a-point) ) ) ) 
( z 1-user : move -to 

(list (truncate (first draw-pt) ) 
(truncate (second draw-pt)))))) 



if-k-k-k-k-k-k-k'k-k-k'kiK'k-k-k-k-k-k-k'k-k-k-k-k-k-k-kic-k-k-k'k'kic-k-k-k-k-k-k-k-k-k-kiK'k-k’k-k 

joystick flavor definition 



(def flavor joystick (( joy-x 0) (joy-y 0) (joy-r 0) ) 

0 

: initable-instance-variables) 



(defmethod (get- joy-value joystick) 

0 

(let* ( (key-value) 

(delta-x 0.2) (delta-y 0.1) (delta-r 0.01)) 
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(setf 

(cond 



(cond 

(cond 

(cond 

(cond 

(list 



key-value ( zl-user : get-keyboard-input ) ) 



( (equal 
( (equal 
( (equal 
( (equal 
( (equal 
( (equal 
( (>= 

( «= 

( (>= 

( (<= 

( (>= 

( «= 



key 

key 

key 

key 

key 

key 



-value 

-value 

-value 

-value 

-value 

-value 



'#\f) 

'#\b) 

'#\r) 

'#\ 1 ) 

'#\=) 



(setf joy-x (+ joy-x delta-x) ) ) 
(setf joy-x (- joy-x delta-x))) 
(setf joy-y (- joy-y delta-y) ) ) 
(setf joy-y (+ joy-y delta-y))) 
(setf joy-r (- joy-r delta-r) ) ) 



Doy-x 

joy-x 

joy-y 

joy-y 

joy-r 

joy-r 



2 ) 

- 2 ) 

1 ) 

- 1 ) 

0 . 1 ) 



(setf joy-r 
(setf joy-x 2) ) 

(setf joy-x -2) ) ) 
(setf joy-y 1)) 

(setf joy-y -1) ) ) 



(+ joy-r delta-r) ) ) ) 



(setf joy-r 0.1)) 

-0.1) (setf joy-r -0.1))) 

((equal key-value '#\Circle) (setf joy-x 0) 
(setf joy-y 0) (setf joy-r 0))) 
joy-x joy-y joy-r (equal key-value ' #\x) ) ) ) 



efmethod (reset joystick) 

0 

(setf joy-x 0) 

(setf joy-y 0) 

(setf joy-r 0) ) 



etf terrain- joystick (make-instance ' joystick) ) 



A****************************************************** 

terrain flavor definition 

AAAAA*A*****AAA*AAA***AAA**AAAAA*AAA*A*A*AAA**AA*****AA 



iefflavor terrain ( (terrain-data (make-array ' (49 49) : initial-element 0) ) 

terrain-height-array terrain-height-list joystick 
(cursor-x) (cursor-y) (max-x) (max-y) ) 

0 

: initable-instance-variables 
: readable-instance-variables ) 



lefmethod (create terrain) 

0 

(init self) 

(modify self) ) 



iefmethod (get-height terrain) 

(a-pos-wrt-earth) 

range 0 =< x <= (first dimension-terrain-height) 

0 =< y <= (second dimension-terrain) . 

(let* ( (dimension-terrain-height (array-dimensions terrain-height-array) ) 
(x-min 0) (x-max (first dimension-terrain-height) ) 

(x (first a-pos-wrt-earth) ) ) 

(if (or (< X x-min) (> x x-max) ) 

-1000 

(let* ( (i-x (floor x) ) 



; get terrain x-index 
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(xl (if (< (- X i-x) 0.5) (- i-x 1) i-x) ) 

(x2 (if (< (- X i-x) 0.5) i-x (+ i-x 1) ) ) 

(xl (if (< xl x-min) 0 xl) ) 

(x2 (if (>= x2 x-max) (- x-max 1) x2) ) 

(zl (aref terrain-height-array xl) ) 

(z2 (aref terrain-height-array x2) ) 

(slope (- z2 zl) ) 

(del-x (- X xl) ) ) 

(+ zl (* slope del-x)))))) 



(defmethod (init terrain) 

0 

; globals : middle-of-screen, eye-space 

(let ( (array-dims (array-dimensions terrain-data) ) ) 

(setf max-x (first array-dims) ) 

(setf max-y (second array-dims) ) 

(setf cursor-x (floor (/ max-x 2) ) ) 

(setf cursor-y (floor (/ max-y 2)))) 

(zl-user : make-robot-window) 

(setf middle-of-screen 

(list (/ ( zl-user : send zl-user ; *robot-window* : inside-width) 2) 

(/ ( zl-user : send zl-user ; *robot-window* : inside-height ) 2))) 

(setf eye-space (eye-trans (list 500 0 0) ) ) 

(read-terrain-height self) 

(draw-terrain self eye-space) 

(display-cursor self) 

( zl-user : make -visible) 

(print "To modify this terrain use keyboard") 

(print "If ready, type any key and return") 

(read) 

(reset terrain- joystick) ) 



(defmethod (in-side-of-whole-terrain terrain) 

(a-pos) 

(let ( (dimension-terrain (array-dimensions terrain-data) ) 



(i-x (floor (first a-pos))) 
(i-y (floor (second a-pos) ) ) ) 



(cond 


( (< 
( « 


i-x 

i-y 


0) nil) 
0) nil) 










( (> 


i-x 


(- (first 


dimension-terrain) 


D) 


nil) 


)) 


( (> 
(T) ) 


i-y 


(- (second 


dimension-terrain) 


D) 


nil) 



(defmethod (modify terrain) 

0 

; external : eye-space 

(do ((radius 500) (alpha 0) (beta 0) (delta 0.0001) 
(joystick-value nil) 

(end-flag nil) ) 

(end-flag (reset joystick) (save-terrain self eye-space) ) 
( zl-user : make- visible) 

(setf joystick-value (get- joy-value joystick)) 

(let ( (x (first joystick-value) ) 

(y (second joystick-value) ) 

(r (third joystick-value) ) 

(fire (fourth joystick-value) ) ) 
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(erase-terrain self) 
(cond 



(fire 

((> X 
(« 
((> 

( « 

( (> 

( (< 
(setf 
(draw 



(setf end-flag t) ) 



delta) (setf alpha (+ alpha 0.05))) 

(- delta)) (setf alpha (- alpha 0.05))) 

delta) (setf beta (+ beta 0.05))) 

(- delta)) (setf beta (- beta 0.05))) 

delta) (setf radius (+ radius 10))) 

(- delta)) (setf radius (- radius 10)))) 
eye-space (eye-trans (list radius alpha beta) ) ) 
terrainl self eye-space) ) ) ) 



efmethod (permitted-cell terrain) 

(terrain-pos) 

(let ( (i-x (floor (first terrain-pos) ) ) ; find terrain index 

(i-y (floor (second terrain-pos) ) ) ) 

(if (in-side-of-whole-terrain self terrain-pos) 

(if (equal (aref terrain-data i-x i-y) 0) ; permitted 

t 

nil)))) 



efmethod (terrain-point terrain) 

I (a-pos-wrt-earth) 

(let* ( (x (first a-pos-wrt-earth) ) 

(y (second a-pos-wrt-earth) ) 

(z (get-height self (list x y) ) ) ) 
(list X y z) ) ) 



etf graph-terrain (make-instance 'terrain 

i : joystick terrain- joystick) ) 



efun init-terrain ( ) 
called to create terrain 
(create graph-terrain) ) 



terrain. display-terrain 

"k’k-k'^k-k-k'ic'k-k^k'k-k-k-k-k-k-k-k-k'k-k'k-k-k'k-k-k-k'k-kir-k-k-k-k'k-k-k-A-k-k'k-k’k-k-k'k-k'k-k-kit'k-k-k-k'k 



efmethod (display-cursor terrain) 

0 

(make-all-permitted self) 

(tv : choose-variable-values 
' ("Choose terrain type" 

(terrain-type "Terrain type" -.choose ("Random" "Manual")))) 
(if (equal terrain-type "Random") 

(random-terrain self) 

(do ((joy-data nil) (x nil) (y nil) (r nil) (fire 



nil) 
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(exit-flag nil) ) 

(exit-flag (erase-cursor self (list cursor-x cursor-y) ) ) 
(zl-user :make-visible) 

(setf joy-data (get- joy-value joystick) ) 

(setf X (- (second joy-data))) (setf y (first joy-data)) 
(setf r (third joy-data) ) (setf fire (fourth joy-data) ) 
(erase-cursor self (list cursor-x cursor-y) ) 

(cond 



(fire 


(setf exit-flag t) ) 




( (> 


X 


0) 


(setf 


cursor-x 


(+ cursor-x 1) ) (if (> cursor-x 
(setf cursor-x max-x) ) ) 


max- 


{ « 


X 


0) 


(setf 


cursor-x 


(- cursor-x 1) ) (if (< cursor-x 
(setf cursor-x 0) ) ) 


0) 


( (> 


y 


0) 


(setf 


cursor-y 


(+ cursor-y 1) ) (if (> cursor-y 
(setf cursor-y max-y) ) ) 


max- 


( « 


y 


0) 


(setf 


cursor-y 


(- cursor-y 1) ) (if (< cursor-y 
(setf cursor-y 0) ) ) 


0) 


( (< 


r 


0) 


(setf 


(aref terrain-data cursor-x cursor-y) 1) ) 




( (> 


r 


0) 


(setf 


(aref terrain-data cursor-x cursor-y) 1))) 



(draw-cursor self (list cursor-x cursor-y) ) 
(draw-obstacles self) 

(reset joystick) ) ) ) 



X) 



y) 



(defmethod (draw-terrain terrain) 

(eye-space) 

; external function: \display . library\move-to-earth, draw-to-earth 

(dotimes (x (+ max-x 1) ) 

(move-to-earth (list x 0 (aref terrain-height-array x) ) ) 
(draw-to-earth (list x max-x (aref terrain-height-array x) ) ) ) 
(dotimes (y (+ max-y 1) ) 

(move-to-earth (list 0 y 0) ) 

(dotimes (x (+ max-x 1)) 

(draw-to-earth (list x y (aref terrain-height-array x) ) ) ) ) ) 



(defmethod (draw-terrainl terrain) 

(eye-space) 

; external function: \display . library \move-to-earth, draw-to-earth 
(do ( (xs (list 0 max-x) (cdr xs) ) 

(x nil) ) 

( (null xs) ) 

(setf X (car xs) ) 

(move-to-earth (list x 0 (aref terrain-height-array x) ) ) 
(draw-to-earth (list x max-x (aref terrain-height-array x) ) ) ) 
(do ( (ys (list 0 max-y) (cdr ys) ) 

(y nil) ) 

( (null ys) ) 

(setf y (car ys) ) 

(move-to-earth (list 0 y 0) ) 

(dotimes (x (+ max-x 1)) 

(draw-to-earth (list x y (aref terrain-height-array x) ) ) ) ) ) 



(defmethod (erase-obstacles terrain) 

0 

; externals : terrain 

; external function: \display . library \move-to-earth, draw-to-earth 
(dotimes (i (first (array-dimensions terrain-data) ) ) 

(dotimes (j (second (array-dimensions terrain-data))) 

(cond ( (equal 1 (aref terrain-data i j) ) 

(move-to-earth (list i j) ) 

(erase-to-earth (list (+ i 1) (+ j 1) ) ) 
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(move-to-earth (list (+ i 1) j) ) 
(erase-to-earth (list i (+ j 1) )))))) ) 



fmethod (erase-terrain terrain) 

0 

tv; sheet -force-access ( z 1-use r : * robot -window * ) 
(send zl-user : *robot-window* ; dear-window) ) ) 



fmethod (make-all-permitted terrain) 

0 

dotimes (i max-x) 

(dotimes (j max-y) 

(setf (aref terrain-data i j) 0) ) ) ) 



fmethod (read-terrain-height terrain) 

0 

(tv; choose -variable -values 
' ("Choose a terrain" 

(terrain-sample "Terrain sample" : choose ("Sample" "Angle" "Custom")))) 
setf terrain-sample "Sample") 
cond ( (equal terrain-sample "Sample") 

(setf terrain-height-list ' ((19 0) (25 1) (35 1.5)))) 

((equal terrain-sample "Angle") 

(tv : choose-variable-values 

'((terrain-angle "Angle in degree" : number)) 

' : label "Slope of terrain") 

(let* ( (angle (* pi (/ terrain-angle 180) ) ) 

(max (* 20 (tan angle) ) ) ) 

(setf terrain-height-list 
(list ' (20 0) 

(list 40 max) 

)))) 

(t (print "Please input terrain height.") 

(setf terrain-height-list (read) ) ) ) 
setf terrain-height-array (make-array (+ max-x 1) ) ) 
let* ( (xl 0) (zl 0) (a-pair) (zz 0) 

(x2 (first (car terrain-height-list) ) ) 

(z2 (second (car terrain-height-list) ) ) 

(slope (/ (- z2 zl) (- x2 xl) ) ) ) 

(setf terrain-height-list (cdr terrain-height-list) ) 

(dotimes (i (+ max-x 1) ) 

(setf zz (-f (* slope (- i xl) ) zl) ) 

(cond ( (equal x2 i) 

(setf xl x2) 

(cond ( (setf a-pair (car terrain-height-list) ) 

(setf terrain-height-list (cdr terrain-height-list) ) 

(setf x2 (first a-pair) ) 

(setf z2 (second a-pair) ) 

(setf zl zz) 

(setf slope (/ (- z2 zl) (- x2 xl) ) ) ) 

(T (setf slope 0) (setf zl zz) ) ) ) ) 

(setf (aref terrain-height-array i) zz) ) ) ) 



fmethod (save-terrain terrain) 
(eye-space) 
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(draw-obstacles self) 

(draw-terrain self eye-space) 

(zl-user : save-terrain-to-terrain-buf f er ) ) 



/ 

; terrain . display-cursor 



.******★★*****★***★★★**********************★***★*******★*** 

r 



(defmethod (draw-cursor terrain) 
( position ) 

(let* ( (x (first position) ) 

(y (second position)) 



<pl 


(list 


(+ 


X 


0.2) 


(+ 


y 


0.2) 


0) ) 


<P2 


(list 


(+ 


X 


0.8) 


<+ 


y 


0.2) 


0) ) 


<p3 


(list 


<+ 


X 


0.8) 


<+ 


y 


0.8) 


0) ) 


<p4 


(list 


(+ 


X 


0.2) 


(+ 


y 


0.8) 


0) ) 



(points (list p2 p3 p4 pi) ) ) 
(move-to-earth pi) 

(do ( (points points (cdr points) ) ) 

((null points) 'done-draw-cursor) 
(draw-to-earth (car points) ) ) ) ) 



(defmethod (draw-obstacles terrain) 

0 

(dotimes (i (first (array-dimensions terrain-data) ) ) 

(dotimes (j (second (array-dimensions terrain-data))) 

(cond ( (equal 1 (aref terrain-data i j) ) 

(move-to-earth 

(list i j (aref terrain-height-array i) ) ) 

(draw-to-earth 

(list (+ i 1) (+ j 1) (aref terrain-height-array (+ i 1) ) ) ) 

(move-to-earth 

(list (+ i 1) j (aref terrain-height-array (+ i 1) ) ) ) 
(draw-to-earth 

(list i (+ j 1) (aref terrain-height-array i) )))))) ) 



(defmethod (erase-cursor terrain) 
( position ) 

(let* ( (x (first position) ) 

(y (second position) ) 



<pl 


(list 


<+ 


X 


0.2) 


(+ 


y 


0.2) 


0) ) 


(P2 


(list 


<+ 


X 


0.8) 


(+ 


y 


0.2) 


0) ) 


(P3 


(list 


(+ 


X 


0.8) 


(+ 


y 


0.8) 


0) ) 


(p4 


(list 


(+ 


X 


0.2) 


(+ 


y 


0.8) 


0) ) 



(points (list p2 p3 p4 pi))) 
(move-to-earth pi) 

(do ( (points points (cdr points) ) ) 

((null points) 'done-erase-cursor) 
(erase-to-earth (car points) ) ) ) ) 
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Lefmethod (random-terrain terrain) 

0 

(let ((a 43411) (b 17) (c 640001) (x nil)) 

(tv : choose-variable-values 

I ' ( (terrain-percent "Obstacles in percentage" : number) 

(terrain-seed "Random number seed" : number) ) 

' : label "How much obstacles on the terrain in percentage? ") 

(setf X terrain-seed) 

(dotimes (i max-x) 

(dotimes (j max-y) 

(if (< (/ (setf X (mod (+ (* a x) b) c) ) c) (/ terrain-percent 100)) 
(setf (aref terrain-data i j) 1))))) 

(draw-obstacles self) 

(zl-user: make-visible)) 



graph-vehicle flavor definition 
********************************************************* 

efflavor graph-vehicle ( (vehicle-points (make-array 28) ) 

(body-points (make-array 10) ) 
(polygons (make-array 13) ) 
(numpolys nil) 

(vertices (make-array 100) ) ) 

0 

: initable-instance-variables ) 



Hefmethod (init-data graph-vehicle) 

0 

(read-vehicle-data self) ) ; read data from disk 



lefmethod (display graph-vehicle) 

(a-H foot -positions) 

(tv: sheet-force-access ( zl-user : *robot-window*) 
(send zl-user : *robot-window* : dear-window) ) 
(zl-user : copy-terrain-to-robot-window) 
(body-pento-wrt-earth self a-H foot -posit ions) 
(draw-vehicle self vehicle-points) 

(zl-user :make-visible) ) 



lefmethod (read-vehicle-data graph-vehicle) 

0 

global variables : vehicle-points, polygons, numpolys, vertices 
format of file : num-of-points num-of-polygons 
( num a-vehicle-point) .... 

( num-of-vertices vertices-number-of-a-polygon) . . . 
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((vehicle-file (open "object : vehicle . data" ) ) 

(numpts (read vehicle-file) ) 

(numvtces 0) (a-polygon nil) ) 

(setf numpolys (read vehicle-file) ) 

(dotimes (i numpts) 

(setf (aref vehicle-points i) (cdr (read vehicle-file)))) 

(dotimes (i 10) 

(setf (aref body-points i) (aref vehicle-points i) ) ) 

(dotimes (i numpolys) 

(setf a-polygon (read vehicle-file) ) 

(setf (aref polygons i) (list numvtces (car a-polygon) ) ) 

(do ( (a-polygon-vertices (cdr a-polygon) (cdr a-polygon-vert ices) ) 
( j 0 (+ j 1) ) ) 

( (null a-polygon-vertices) ) 

(setf (aref vertices (+ numvtces j) ) 

(- (first a-polygon-vertices) 1) ) ) 

(setf numvtces (+ numvtces (car a-polygon) ) ) ) 

(close vehicle-file) ) ) 



(setf graph-asv (make-instance 'graph-vehicle)) 



graph-vehicle . display 

■k-k-k-k^-k-k-k-k-k-k-k-k-k^-k-k-k-k-k-k^k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-kir-k-k-k-k-k-k-k-k-kir-k-k-k-k-k-k-k 



(defmethod (body-pento-wrt-earth graph-vehicle) 

( a-H foot-positions ) 

; library : transform 

(let ((si 0.6616) (s2 0.945) (s3 3.308) (1 0.8133) (m 1.0467) 

(hipx-list '(5.1667 5.1667 0.0 0.0 -4.9167 -4.9167)) 
(hipy-list '(1.62 -1.62 1.62 -1.62 1.62 -1.62)) 

(signl-list ' (1 -1 1-1 1 -1) ) 

(sign2-list '(1 1 1 1-1-1))) 

(transform-body-points self a-H body-points) 

(do ( (positions foot-positions (cdr positions) ) 

(hipx-list hipx-list (cdr hipx-list) ) 

(hipy-list hipy-list (cdr hipy-list) ) 

(signl-list signl-list (cdr signl-list) ) 

(sign2-list sign2-list (cdr sign2-list) ) 

(i 0 (+ i 1))) 

( (null positions) nil) 

(let* ( (foot-pos (car positions) ) 

(hipx (car hipx-list)) (hipy (car hipy-list)) 

(signl (car signl-list)) (sign2 (car sign2-list) ) 

(px (- (first foot-pos) hipx) ) 

(py (” (second foot-pos) hipy) ) 

(pz (third foot-pos) ) 

(theta (vehicle-theta py pz m signl) ) 

(dm (vehicle-dm px sign2) ) 

(dl (vehicle-dl py pz m 1) ) 

(top-pos nil) (knee-pos nil) ) 

(setf top-pos 

(transform a-H 

(vehicle-top-pos hipx hipy m 1 dl theta signl) ) ) 
(setf knee-pos 

(transform a-H 



isplay . lisp 



Wed Mar 30 15:27:49 1988 



11 



(vehicle-knee-pos hipx hipy m 1 si s2 s3 
dl dm theta signl sign2) ) ) 



(setf 


foot-pos (transform a 


-H 


foot-pos) ) 


(setf 


(aref vehicle-points 
top-pos) 


( + 


10 


(* 3 


i) ) ) 


(setf 


(aref vehicle-points 
knee-pos) 


( + 


11 


(* 3 


i) ) ) 


(setf 


(aref vehicle-points 
foot-pos) ) ) ) ) 


( + 


12 


(* 3 


i) ) ) 



[efmethod (draw-vehicle graph-vehicle) 

( vehicle-points ) 

jglobal variables : polygons^ numpolys^ vertices 

(dotimes (i numpolys) 

(let ( (start (first (aref polygons i) ) ) 

(num-vertices (second (aref polygons i) ) ) ) 
(move-to-earth (aref vehicle-points 
(aref vertices start) ) ) 

! (dotimes (j num-vertices) 

(draw-to-earth-d (aref vehicle-points 
j (aref vertices (-f start j) ) ) ) ) 

) ) ) 



I graph-vehicle . display . body-pento-wrt-earth 
★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ 



efmethod (transform-body-points graph-vehicle) 
(a-H body-points) 
globals : vehicle-points 
library : transform 
(dotimes (i 10) 

I (setf (aref vehicle-points i) 

(transform a-H (aref body-points i) ) ) ) ) 



:.efun vehicle-dl (py pz m 1) 

i (/ (- (sqrt (+ (* py py) (* pz pz) (- (* m m) ) ) ) 1) 

i 4) ) 



I 



lefun vehicle-dm (px sign2) 
(* sign2 (/ px 5) ) ) 



lefun vehicle-knee-pos (hipx hipy m 1 si s2 s3 
dl dm theta signl sign2) 

(let* ( (numer (*f (* si si) (- (* s2 s2) ) (* dl dl) (* dm dm))) 
(denom (* 2 si (sqrt (+ (* dl dl) (* dm dm) ) ) ) ) 

(beta (acos (/ numer denom) ) ) 

(alpha (- (/ pi 2) (atan dm dl) beta) ) 
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(sina (sin alpha) ) (cosa (cos alpha) ) 

(sint (sin theta) ) (cost (cos theta) ) 

(temp (- (* s3 sina) (- dl 1) ) ) 

(xk (+ (* sign2 s3 cosa) hipx) ) 

(yk (+ (* signl (+ (* temp sint) (* m cost) ) ) hipy) ) 
(zk (- (+ (* temp cost) (* m sint) ) ) ) ) 

(list xk yk zk) ) ) 



(defun vehicle-theta (py pz m signl) 

(let* ( (anglel (atan (* signl py) (* -1 pz) ) ) 
(angle2 (atan m (sqrt (*f (* py py) 

(* pz pz) 

(- (* m m) ) ) ) ) ) ) 

(- anglel angle2) ) ) 



(defun vehicle-top-pos (hipx hipy m 1 dl theta signl) 
(let* ( (xt hipx) 

(1-dl (- 1 dl)) 

(sina (sin theta) ) 

(cosa (cos theta) ) 

(yt (+ (* signl (+ (* m cosa) (* 1-dl sina))) hipy)) 
(zt (- (* m sina) (* 1-dl cosa) ) ) ) 

(list xt yt zt) ) ) 
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; Mode: LISP; Package: LEG; Syntax: Common- lisp; Base: 10 

executor flavor definition 



efflavor executor 

(leg-pos-wrt-body desired-foothold-pos-wrt -earth 

time command owner sensor (lift-height 1.4) 
(T1 0.6) (T2 1.0) (T3 0.4) (T4 0.6) 

(planned-contact-time 0.4) self-time 
( sampling-time 0.1) ready-pos 

HI inv-Hl body-trans-ratel body-rotate-ratel ) 

0 

: init able- instance- variables) 



sfmethod ( set-desired-pos executor) 

(a-pos) 

(setf desired-foothold-pos-wrt-earth a-pos) ) 



method (get-desired-pos executor) 

0 

si red- foothold-pos-wrt -earth) 



Bfmethod (send- command executor) 
(a-command) 

(setf command a-command) ) 



sfmethod (set-time executor) 
(a-time) 

(setf time a-time) ) 



sfmethod (leg-pos-wrt-body executor) 

0 



leg-pos-wrt-body) 



sfmethod (move executor) 

(H inv-H body-trans-rate body-rotate-rate) 
(setf HI H) 

(setf inv-Hl inv-H) 

(setf body-trans-ratel body-trans-rate) 

(setf body-rotate-ratel body-rotate-rate) 

(cond ( (equal command ' ready) 

(move-in-ready self) ) 

((equal command 'advance) 

(move-in-advance self) ) 

((equal command 'descent) 

(move-in-descent self) ) 

((equal command 'contact) 
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(move-in-contact self) ) 
((equal command 'support) 
(move-in-support self) ) 
((equal command 'lift) 
(move-in-lift self) ) 
((equal command 'return) 
(move-in-return self) ) 

) 



(defmethod (move-in-contact executor) 
0 



(let ( (leg-velocity-wrt-body ( f ind-velocity-wrt-body self) ) ) 
(setf leg-pos-wrt-body 

(vectadd (magvect sampling-time leg-velocity-wrt-body) 
leg-pos-wrt-body) ) ) ) 



(defmethod ( f ind-velocity-wrt-body executor) 

0 

; returns f oot-velocity-wrt-body 

; velocity = - ( body-trans-rate + body-rotate-rate X leg-pos ) 

; globals v : body-trans-ratel , body-rotate-ratel 
; lib : vectsub, vectadd, crossprod 
(vectsub '(000) 

(vectadd body-t rans-ratel 

(crossprod body-rotate-ratel leg-pos-wrt-body) ) ) ) 



(defmethod (move-in-advance executor) 

0 

(let ( (desired-pos (desired-advance-pos-wrt-body self) ) 
(dt (- T1 time) ) ) 

(move-del self desired-pos leg-pos-wrt-body dt) ) 

(setf self-time 0.0)) 



(defmethod (desired-advance-pos-wrt-body executor) 

0 

; a-pos is desired-stepping-pos-wrt-earth 
; returns desired-pos-wrt-body in deploy state 
; global variable : HI, inv-Hl 

; global function : to-earth-transf orm, to-body-transf orm, f ind-terrain-hegiht 
(let* ( (desired-pos-wrt-earth desired-f oothold-pos-wrt-earth) 

(terrain-height (third (terrain-point owner desired-pos-wrt-earth) ) ) 
(desired-pos-height-wrt-earth (+ terrain-height lift-height) ) 
(pos-wrt-earth (list (first desired-pos-wrt-earth) 

(second desired-pos-wrt-earth) 
desired-pos-height-wrt-earth) ) ) 
(to-body-transform inv-Hl pos-wrt-earth) ) ) 



(defmethod (move-in-descent executor) 

0 

; global function : to-body-transform 
; global variables : inv-Hl 

(let ( (dt (- planned-contact-time self-time) ) ) 
(if (< dt 0.05) 

(setf leg-pos-wrt-body (to-body-transform 
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inv-Hl desired-foothold-pos-wrt-earth) ) 

(move-del self 

(to-body-transform inv-Hl desired-foothold-pos-wrt-earth) 
leg-pos-wrt-body dt) ) ) ) 



sfmethod (move-in-descent executor : after) 

0 

(setf self-time (+ self-time sampling-time) ) ) 



ifmethod (move-del executor) 

(desired-pos present-pos dt) 
jet new leg-pos depending on the arguments 
.ib : vectadd, magvect 
(if (< dt 0.05) 

(setf leg-pos-wrt-body desired-pos) 

(let* ( (inv-time-dif f (/ 1 dt) ) 

(del (vectsub desired-pos present-pos) ) 

(velocity (magvect inv-time-dif f del) ) ) 

(setf leg-pos-wrt-body 

(vectadd present-pos (magvect sampling-time velocity) ) ) ) ) ) 



‘fmethod (move-in-lift executor) 

0 

[let* ( (dt (- T3 time) ) 

(desired-pos ( lif t-pos-desired self) ) 

(2 (third desired-pos) ) ) 

(move-del self desired-pos leg-pos-wrt-body dt) 

(setf ready-pos 

(list (first ready-pos) (second ready-pos) z) ) ) ) 



ifmethod (lift-pos-desired executor) 

:eturens position-wrt-body which will be at the end of lift state, 
jlobal f : to-body-transform, 
jlobal V : inv-Hl 
0 

[let* ( (leg-pos-wrt-earth (to-earth-transform HI leg-pos-wrt-body) ) 

(desired-height (+ lift-height (third (terrain-point owner leg-pos-wrt-earth) ) ) ) ) 
(to-body-transform inv-Hl (list (first leg-pos-wrt-earth) 

(second leg-pos-wrt-earth) 
desired-height) ) ) ) 



ifmethod (move-in-ready executor) 

0 

(setf leg-pos-wrt-body ready-pos) ) 



ifmethod (move-in-return executor) 

0 

lodifying leg-pos-z is redundent but it can correct disturbance by itself, 
(let ( (dt (- T4 time)) 

(desired-pos ready-pos) ) 

(move-del self desired-pos leg-pos-wrt-body dt) ) ) 
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(defmethod (move-in-support executor) 

0 

; globals : body-trans-ratel, body-rotate-ratel 
; lib : vectadd, magvect 

; In general terrain, leg-pos-z should be updated by real terrain height . 
(let ( (leg-velocity-wrt-body (f ind-velocity-wrt-body self) ) ) 

(setf leg-pos-wrt-body 

(vectadd (magvect sampling-time leg-velocity-wrt-body) 
leg-pos-wrt-body) ) ) ) 



(defmethod (init executor) 

(leg-name init-H) 

(setf sensor (leg-contact-sensor owner) ) 

(let ( (x (aref init-H 0 3) ) 

(y (aref init-H 1 3) ) 

(z (aref init-H 2 3) ) ) 

(cond ((equal leg-name 'legl) 

(setf ready-pos '{ 5 3 -4) ) 

(setf leg-pos-wrt-body (list 6 3 (- z))) 
(setf desired-f oothold-pos-wrt-earth (list 
((equal leg-name 'leg2) 

(setf ready-pos ' ( 5 -3 -4) ) 

(setf leg-pos-wrt-body (list 5-3 (~ z) ) ) 

(setf desired-foothold-pos-wrt-earth (list 
((equal leg-name 'leg3) 

(setf ready-pos ' ( 0 3 -4) ) 

(setf leg-pos-wrt-body (list 0 3 (- z) ) ) 
(setf desired-foothold-pos-wrt-earth (list 
((equal leg-name 'leg4) 

(setf ready-pos ' ( 0 -3 -4) ) 

(setf leg-pos-wrt-body (list 0-3 (- z) ) ) 
(setf desired-foothold-pos-wrt-earth (list 
((equal leg-name 'leg5) 

(setf ready-pos ' (-5 3 -4) ) 

(setf leg-pos-wrt-body (list -5 3 (- z) ) ) 

(setf desired-foothold-pos-wrt-earth (list 
((equal leg-name ' leg6) 

(setf ready-pos ' (-5 -3 -4) ) 

(setf leg-pos-wrt-body (list -5 -3 (- z) ) ) 
(setf desired-foothold-pos-wrt-earth (list 



(+ X 6) 



(+ X 5) 



<+ X 0) 



(+ X 0) 



(- X 5) 



(- X 5) 



(+ y 3) 0))) 

(- y 3) 0))) 

(+ y 3) 0))) 

(- y 3) 0))) 

(+ y 3) 0))) 

(- y 3) 0))))) 
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; Mode: LISP; Syntax: Coiranon-lisp; Package: LEG 



ef flavor foothold-finder (sixteen-footholds 

four-lines tkm-calculator 
(no-cell-available-flag nil) 
(TKM-margin 0.4) owner) 

0 

: initable-instance-variables) 



efmethod (init foothold-finder) 
(leg-name) 

(cond ((equal leg-name 'legl) 
(setf sixteen-footholds 





' ( ( 7.3 4.3) (7.3 3.3) 




( 7.3 


2.3) 


( 


7.3 


1.3) 




( 6.3 4.3) ( 6.3 3.3) 




( 6.3 


2.3) 


( 


6.3 


1.3) 




( 5.3 4.3) ( 5.3 3.3) 




( 5.3 


2.3) 


( 


5.3 


1.3) 




( 4.3 4.3) ( 4.3 3.3) 




( 4.3 


2.3) 


( 


4.3 


1.3) ) ) 


(setf 


four-lines 
















' ( ( (0 0.3420 -0.9397) ( 


8 


.0832 


2.7339 


0)) 






( (0 -0.3420 -0.9397) ( 


8 


.0832 


2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


3 


.4167 


2.7339 


0) ) 






( (0 0.3420 -0.9397) ( 


3 


.4167 


2.7339 


0))))) 


( (equal 


leg-name 'leg2) 














(setf 


sixteen- footholds 
















' ( ( 7.3 -4.3) ( 7.3 -3.3) 




( 7.3 


-2.3) 


( 


7.3 


-1.3) 




( 6.3 -4.3) ( 6.3 -3.3) 




( 6.3 


-2.3) 


( 


6.3 


-1.3) 




( 5.3 -4.3) ( 5.3 -3.3) 




( 5.3 


-2.3) 


( 


5.3 


-1.3) 




( 4.3 -4.3) ( 4.3 -3.3) 




( 4.3 


-2.3) 


( 


4.3 


-1.3) ) ) 


(setf 


four-lines 
















' ( ( (0 0.3420 -0.9397) ( 


8 


.0832 


-2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


8 


.0832 


-2.7339 


0)) 






( (0 -0.3420 -0.9397) ( 


3 


. 4167 


-2.7339 


0)) 






( (0 0.3420 -0.9397) ( 


3 


.4167 


-2.7339 


0) ) ) ) ) 


( (equal 


leg-name ' leg3 ) 














(setf 


sixteen- footholds 
















' ( ( 1.5 4.3) ( 1.5 3.3) 




( 1.5 


2.3) 


( 


1.5 


1.3) 




( 0.5 4.3) ( 0.5 3.3) 




( 0.5 


2.3) 


( 


0.5 


1.3) 




(-0.5 4.3) (-0.5 3.3) 




(-0.5 


2.3) 


(- 


•0.5 


1.3) 




(-1.5 4.3) (-1.5 3.3) 




(-1.5 


2.3) 


(- 


■1.5 


1.3) ) ) 


(setf 


four-lines 
















' ( ( (0 0.3420 -0.9397) ( 


2 


.2915 


2.7339 


0)) 






( (0 -0.3420 -0.9397) ( 


2 


.2915 


2.7339 


0) ) 






( (0 -0.3420 -0.9397) (- 


2 


.2915 


2.7339 


0) ) 






( (0 0.3420 -0.9397) (- 


2 


.2915 


2.7339 


0))))) 


( (equal 


leg-name 'leg4) 














( setf 


sixteen- foot holds 
















' ( ( 1.5 -4.3) ( 1.5 -3.3) 




( 1.5 


-2.3) 


( 


1.5 


-1.3) 




( 0.5 -4.3) ( 0.5 -3.3) 




( 0.5 


-2.3) 


( 


0.5 


-1.3) 




(-0.5 -4.3) (-0.5 -3.3) 




(-0.5 


-2.3) 


(- 


•0.5 


-1.3) 




(-1.5 -4.3) (-1.5 -3.3) 




(-1.5 


-2.3) 


(- 


-1.5 


-1.3) ) ) 


(setf 


four-lines 
















' ( ( (0 0.3420 -0.9397) ( 


2 


.2915 


-2.7339 


0) ) 






( (0 -0.3420 -0.9397) ( 


2 


.2915 


-2.7339 


0) ) 






( (0 -0.3420 -0.9397) (- 


2 


.2915 


-2.7339 


0)) 






( (0 0.3420 -0.9397) (- 


2 


.2915 


-2.7339 


0))))) 


( (equal 


leg-name 'leg5) 














(setf 


sixteen- footholds 
















' ( (-4.0 4.3) (-4.0 3.3) 




(-4.0 


2.3) 


(- 


-4.0 


1.3) 




(-5.0 4.3) (-5.0 3.3) 




(-5.0 


2.3) 


(- 


-5.0 


1.3) 




(-6.0 4.3) (-6.0 3.3) 




(-6.0 


2.3) 


(- 


•6.0 


1.3) 




(-7.0 4.3) (-7.0 3.3) 




(-7.0 


2.3) 


(- 


-7.0 


1.3) ) ) 
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(setf four-lines 



' ( ( (0 0.3420 - 0 . 9397 ) 

( (0 - 0.3420 - 0 . 9397 ) 
( (0 - 0.3420 - 0 . 9397 ) 
( (0 0.3420 - 0 . 9397 ) 

((equal leg-name 'leg6) 

(setf sixteen-footholds 



(- 3.3332 

(- 3.3332 

(- 7.8332 

(- 7.8332 



' ( (- 4.0 - 4 . 3 ) 
(- 5.0 - 4 . 3 ) 
(- 6.0 - 4 . 3 ) 
(- 7.0 - 4 . 3 ) 



(- 4.0 - 3 . 3 ) (- 4.0 

(- 5.0 - 3 . 3 ) (- 5.0 

(- 6.0 - 3 . 3 ) (- 6.0 

(- 7.0 - 3 . 3 ) (- 7.0 



(setf four-lines 

' ( ( (0 0.3420 - 0 . 9397 ) 

( (0 - 0.3420 - 0 . 9397 ) 
( (0 - 0.3420 - 0 . 9397 ) 
( (0 0.3420 - 0 . 9397 ) 



(- 3.3332 

(- 3.3332 

(- 7.8332 

(- 7.8332 



2.7339 0 ) ) 

2.7339 0 ) ) 

2.7339 0 ) ) 

2.7339 0 ) ) ) ) ) 



- 2 . 3 ) (- 4.0 - 1 . 3 ) 

- 2 . 3 ) (- 5.0 - 1 . 3 ) 

- 2 . 3 ) (- 6.0 - 1 . 3 ) 

- 2 . 3 ) (- 7.0 - 1 . 3 ) ) ) 

- 2.7339 0 ) ) 

- 2.7339 0 ) ) 

- 2.7339 0 ) ) 

- 2.7339 0 ) ) ) ) ) 



) 

(setf tkm-calculator (leg-tkm-calculator owner) ) 

) 



(defmethod (find-foothold foothold-finder) 

(H6 inv-H6 body-t rans-ratelO body-rotate-ratelO 
estimated- support-plane) 

; returns ( (max-foothold max-tkm) (foothold-list) (tkm-list) ) 

; all points are wpt body coordinate system. 

(let* ( (estimated-support -plane-wrt -body 

(plane-transform estimated-support-plane H6) ) 

( four-points ( four-point s-on- support -plane 
self 

four-lines estimated-support-plane-wrt-body) ) 

(possible -foot holds (get -possible -foot holds 

self 

(estimate- foot holds 
self 

f our-points est imated-support -plane-wrt -body ) 
H6 inv-H6) ) ) 

( get -foot hold- wit h-max-TKM 
self 

possible-footholds H6 

body-trans-ratelO body-rotate-ratelO) ) ) 






; foothold-finder . find-foothold 

.n***yr*********yr****************************************n 



(defmethod (estimate-footholds foothold-finder) 

( four-point s-wrt-body est imated-support-plane-wrt-body ) 
; returns estimate-f ootholds-wrt-body 

(do* ( (footholds sixteen-f ootholds (cdr footholds) ) 
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(out-footholds nil) 

(a-foothold nil) ) 

( (null footholds) 

(get-points-on-support-plane out-footholds estimated-support-plane-wrt-body) ) 
(setf a-foothold (car footholds) ) 

(if (in-side-of-polygon a-foothold 

(pick-two-dimensions four-point s-wrt-body) ) 

(setf out-footholds (cons a-foothold out-footholds))))) 



“fmethod (f our-points-on-support-plane foothold-finder) 

(four-lines est imated- support -plane- wrt -body) 
returns four points which are intersected by four-lines on 
estimated- support -plane -wrt -body 
tiath lib: plane-intersection 
(do* ( (lines four-lines (cdr lines) ) 

(points nil) ) 

( (null lines) points) 

(setf points (cons (plane-intersection (car lines) 

est imated- support -plane -wrt -body) 



points) ) ) ) 



sfmethod (get-foothold-with-max-TKM foothold-finder) 

(possible-footholds H 

body-trans-rate body- rot ate- rate) 
returns ( (max-foothold max-tkm) (foothold-list) (tkm-list) ) 

3ets no-cell-available-flag 

:eal-f ootholds is really possible footholds 

(do ( (footholds possible-footholds (cdr footholds) ) 

(max-foothold nil) (a-foothold nil) (TKM-list nil) (a-TKM nil) 
(real-footholds nil) (max-TKM -100.0)) 

( (null footholds) 

(setf no-cell-available-flag (< max-TKM TKM-margin) ) 

(if (>= max-TKM TKM-margin) 

(make-output-f orm 

max-foothold max-TKM real-footholds TKM-list H) 
nil) ) 

(setf a-foothold (car footholds) ) 

(setf a-TKM (find-tkm tkm-calculator 

a-foothold body-trans-rate body-rotate-rate) ) 

(if a-TKM 

(progn (setf TKM-list (cons a-TKM TKM-list) ) 

(setf real-footholds (cons a-foothold real-footholds) ) 
(if (> a-TKM max-TKM) 

(progn (setf max-TKM a-TKM) 

(setf max-foothold a-foothold) )))))) 



sfmethod (get-possible-f ootholds f oothold-f inder) 
(estimated-footholds H inv-H) 
returns possible-footholds wrt body 
(to-body-transform inv-H 

(find-possible-footholds self 

(to-earth-transform H estimated-footholds) ) 



') 



4 
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; foothold-finder . estimate-foothold 

r 



(defun check-polarity (pointl point2 pointS) 

(let* ( (vectl (vectsub point2 pointl) ) 

(vect2 (vectsub pointS pointl) ) ) 

(if (not (third vectl) ) 

(progn (setf vectl (reverse (cons 0 (reverse vectl) ) ) ) 

(setf vect2 (reverse (cons 0 (reverse vect2) ) ) ) ) ) 
(crossprod vectl vect2) ) ) 



(defun get-point s-on-support -plane (points est imat ed-support-plane-wrt-body) 

; returns intersection points with support plane in z-body direction. 

; math lib: plane-intersection 

(do* ( (points points (cdr points) ) 

(out-points nil) ) 

( (null points) out-points) 

(setf out-points (cons (plane-intersection 

(make-1 ine-to-get -point -on- support -plane 
(car points) ) 

estimated-support-plane-wrt-body) out-points) ) ) ) 



(defun in-side-of-polygon (a-point polygon-points) 

; polygon-points must be convext-polygon and in order & two dimensional points, 
(do* ( (first-points polygon-points (cdr first-points) ) 

(second-points (reverse (cons (car first-points) 

(reverse (cdr first-points) ) ) ) 

(cdr second-points) ) 

(signs nil) (first-point nil) (second-point nil) ) 

( (null first-points) ( Scime-polarity signs) ) 

(setf first-point (car first-points) ) 

(setf second-point (car second-points) ) 

(setf signs (cons (check-polarity first-point second-point a-point) 
signs) ) ) ) 



(defun make-line-to-get-point-on-support-plane (a-point) 

; a-point is two dimensional point. 

/ returns a-line ( ( z-direct ion) (a-point -100) ) 

(list '(001) (list (first a-point) (second a-point) -100) ) ) 



(defun pick-two-dimensions (points) 

(if (listp (first points) ) 

(do* ( (points points (cdr points) ) ; more than one point case 

(a-point nil) 

(out-points nil) ) 

( (null points) out-points) 

(setf a-point (car points) ) 

(setf out-points (cons (list (first a-point) (second a-point)) 

out-points) ) ) 

(list (first points) (second points) ) ) ) ; one point case 
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efun same-polarity (signs) 

(do ((signs (cdr signs) (cdr signs)) 

(first-sign (plusp (third (car signs) ) ) ) 

(same T) ) 

( (null signs) same) 

(if (not (equal first-sign (plusp (third (car signs) ) ) ) ) 
(setf same nil) ) ) ) 



foothold-finder . find-foothold . get - foot hold- wit h-MAX-t Jon 



efun maJce-output-f orm 

(max-foothold max-TKM possible-footholds TKM-list H) 
output-form : ( ( f oothold-with-max-tkm tJcm) 

(leg-pro jected-permit ted- footholds) 
(leg-pro jected-TKM-list) ) 
output footholds are in earth coordinate, 
math lib : to-earth-transf orm 

(list (list (to-earth-transf orm H max-foothold) max-TKM) 
(to-earth-transf orm H possible-footholds) 

TKM-list) ) 



foot hold- finder . select- foot hold . get -possible -foothold 



.efmethod (find-possible-footholds foothold-finder) 
(estimated-footholds-wrt -earth) 
returns pos s ible- footholds -wrt -earth 
luse vision 

I (do"*" ( (footholds estimated-f ootholds-wrt-earth (cdr footholds) ) 
(a-foothold nil) (t-cell nil) (out-footholds nil) ) 

( (null footholds) (unique-f ootholds-only out-footholds) ) 
(setf a-foothold (car footholds) ) 

(setf t-cell (get-center-of-digitized-terrain-cell a-foothold) ) 
(if (permitted-cell owner t-cell) 

(setf out-footholds 

(cons (terrain-point owner t-cell) 
out-footholds) ) ) ) ) 



lefun get-center-of-digitized-terrain-cell (a-foothold) 
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; cell resolution is 1 foot by 1 foot 

(list (+ (floor (first a-foothold) ) 0.5) 

(+ (floor (second a-foothold)) 0.5))) 



(defun unique-footholds-only (mixed-footholds) 

(do’^ ((footholds mixed-footholds (cdr footholds)) 
(out-footholds nil) 

(a-foothold nil) ) 

( (null footholds) out-footholds) 

(setf a-foothold (car footholds) ) 

(if (not (member a-foothold out-footholds :test 'equal)) 

(setf out-footholds (cons a-foothold out-footholds) ) ) ) ) 
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; mode: lisp; syntax : zetalisp; Package: USER 

low level graph routines 

/^*******yc*yr************yryc*ycyr*yryryryr*yc***yc****yr************ 



^fvar *robot-display-window* nil) 
ifvar *robot-display-window-array* nil) 
sfvar *robot-window* nil) 
ifvar *robot-window-array* nil) 

5 fvar *robot-window-width* nil) 
ifvar *robot-window-height * nil) 

5 fvar *terrain-buf f er* nil) 
ifvar *terrain-buf f er-array* nil) 
sfvar *max-y* nil) 
ifvar *start-point * nil) 



sfun kill-all-windows ( ) 

(send *robot-display-window* :kill) 
(send *robot-window* :kill) 

(send *terrain-buf f er* :kill)) 



jfun copy-terrain-to-robot-window () 

(tv : sheet-force-access (*robot-window*) 

(send * robot -window* :bitblt 

tv:alu-ior *robot-window-width* *robot-window-height 
*terrain-buf fer-array* 2 2 0 0) ) ) 



ifun draw-to (a-point a-window) 
jlobal variables : *start-point * 

(tv : sheet-force-access (a-window) 

(send a-window ' :draw-line (first *start-point *) 
(- *max-y* (second *start-point * ) ) 

(first a-point) 

(- *max-y* (second a-point)) tv:alu-ior)) 
(setq *start-point * a-point) ) 



ifun draw-to-d (a-point a-window) 
global variables : *start-point * 

(tv: sheet-force-access (a-window) 

(send a-window ' : draw-fat-line (first *start-point * ) 
(- *max-y* (second *start-point *) ) 

(first a-point) 

(- *max-y* (second a-point)) tv:alu-ior)) 
ksetq *start-point* a-point) ) 



jfun erase-to (a-point a-window) 
jlobal variables : *start-point * 

(tv; sheet-force-access (a-window) 

(send a-window ':draw-line (first *start-point*) 
(- *max-y* (second *start-point * ) ) 

(first a-point) 
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(- *max-y* (second a-point) ) tv : alu-andca) ) 
(setq * 3 tart -point * a-point)) 



(defun get-keyboard-input ( ) 

; This is not for the graphics, but this function uses Zeta LISP. 

; This is the reason why this function is in Zeta graphic package, 
(send terminal-io : tyi-no-hang) ) 



(defun make-robot-window () 

( setq *robot-display-window* (tv :make-window ' tv : window 

' :blinker-p nil 
':edges-from rmouse 
' ’.borders 2 

label ”robot-display-window” 

' :name ”robot-display-window” 

' : save-bits t 
' : expose-p t ) ) 

(let* ( (r-w (send *robot-display-window* : width)) 

(r-h (send *robot-display-window* iheight)) 

(r-x nil) (r-y nil) ) 

(multiple-value (r-x r-y) (send *robot-display-window* iposition) ) 

(setq *robot-window* (tv :make-window 'tvrwindow 
' :position (list r-x r-y) 

' : width r-w 
' : height r-h 
' :blinker-p nil 
' : borders 2 

'.•label "robot -window” 

' : name "robot-window" 

' : save-bits t 
' :expose-p nil) ) 

(setq *terrain-buf f er* (tv :make-window 'tvrwindow 

' Iposition (list r-x r-y) 

' rwidth r-w 
' : height r-h 
' :blinker-p nil 
' : borders 2 

': label "terrain-buffer" 

' : name "terrain-buffer" 

' : save-bits t 
' : expose-p nil) ) 

(setq *max-y* (send *robot-window* : inside-height )) ) 

(setq *robot-display-window-array* (send *robot-display-window* :bit-array)) 
(setq *robot-window-array* (send *robot-window* :bit-array) ) 

(setq *robot-window-width* (send *robot-window* : inside-width) ) 

(setq *robot-window-height * (send *robot-window* : inside-height ) ) 

(setq *terrain-buf f er-array* (send *terrain-buf f er* :bit-array) ) ) 



(defun make-visible () 

(send *robot-display-window* rbitblt 

tv:alu-seta *robot-window-width* *robot-window-height * 
*robot-window-array* 2200)) 



(defun move-to (a-point) 

; global variables : *start-point * 

; This function just changes *start-point * . 
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•fun save-terrain-to-terrain-buf fer () 
tv; sheet-force-access ( *terrain-buf f er* ) 

(send *terrain-buf f er * :bitblt 

tv:alu-seta *robot -window-width* *robot-window-height * 
*robot-window-array* 2 2 0 0) ) ) 
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. Mode: LISP; Syntax: Corranon-lisp; Package: 

sfflavor H-calculat or ( (sampling-time 0.1) H 

old-H) 



0 

: initable-instance-variables) 



BODY 



efmethod (init H-calculator) 

0 

library fucntion : ident 
(setf H (ident)) 

(setf (aref H 0 3) 6.5) 

(setf (aref H 1 3) 19.5) 
(setf (aref H 2 3) 5.4) 

H) 



sfmethod (new-H H-calculator) 

(body-trans-rate body-rot ate- rate) 

(setf H 

(orthogonalization 

(get-new-H 

H 

(get-del-H 

H 

(get-delta body-trans-rate body-rotate-rate sampling-time) ) ) ) ) ) 



efmethod (save H-calculator) 

0 

(setf old-H H)) 



efmethod (restore H-calculator) 

0 

(setf H old-H) ) 






H-calculator . new-H 



efun get-delta (body-trans-rate body- 
(let* ( (del-trans-x (* (first body- 
(del-trans-y 
(del-trans -2 
(del-rotate-x 
(del-rotate-y 
(del-rotate -2 
(list (list del-trans-x del-trans- 
(list del-rotate-x del-rotate 



(* 

(* 

(* 

(* 

(* 



(second body- 
(third body- 
(first body- 
(second body- 
(third body- 



rotate-rate sampling-time) 
trans-rate) sampling-time) ) 
trans-rate) sampling-time) ) 
trans-rate) sampling- time) ) 
rotate-rate) sampling-time) ) 
rotate-rate) sampling-time) ) 
rotate-rate) sampling-time) ) ) 
y del-trans-z) 

-y del-rotate-z) ) ) ) 



efun get-del-H (H delta-trans-rotate) 



h-calculator . lisp 



Wed Mar 30 15:27:57 1988 



2 



math lib : ident 

(let* ( (H-del (ident)) ; initialze identity matirix 

(delta-trans (first delta-trans-rotate) ) 
(delta-rotate (second delta-trans-rotate) ) ) 



(set f 


(aref 


H-del 


0 


0) 


0) 




(setf 


(aref 


H-del 


1 


0) 


(third 


delta-rotate) ) 


(setf 


(aref 


H-del 


2 


0) 


(- (second delta-rotate) ) 


(setf 


(aref 


H-del 


0 


1) 


(- (third delta-rotate) ) ) 


(setf 


(aref 


H-del 


1 


1) 


0) 




(setf 


(aref 


H-del 


2 


1) 


(first 


delta-rotate) ) 


(setf 


(aref 


H-del 


0 


2) 


(second 


delta-rotate) ) 


(setf 


(aref 


H-del 


1 


2) 


(- (first delta-rotate))) 


(setf 


(aref 


H-del 


2 


2) 


0) 




(setf 


(aref 


H-del 


0 


3) 


(first 


delta-trans) ) 


(setf 


(aref 


H-del 


1 


3) 


(second 


delta-trans) ) 


(setf 


(aref 


H-del 


2 


3) 


(third 


delta-trans) ) 


(setf 


(aref 


H-del 


3 


3) 


0) 




(matrixmult 


H H-del) ) ) 







(defun get-new-H (H del-H) 
(matrixadd H del-H) ) 
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Package: LEG; Mode: LISP; Syntax: Common-lisp; Base: 10 

★ A**************************:*:********************************* 



leg flavor definition 



ifflavor leg (name owner plan-machine control-machine 

executor contact-sensor tkm-calculator 
foothold-finder exchanged-leg 
foothold tkm foothold-list tkm-list tkm-p 
reserved-foothold reserved-tkm) 

0 

init able -instance- variables 
readable-instance- variables) 



fmethod (init leg) 
(H) 



(setf 
(set f 
(setf 
(setf 
(setf 
(setf 
(setf 
(init 
(init 
(init 
(init 
(init 



contact -sensor 
executor 
control -machine 
plan-machine 
tkm-calculator 
foot hold- finder 



(make-instance 
(make- instance 
(make-instance 
(make-instance 
(make- instance 
(make-instance 



foothold (init executor name H) ) 
contact-sensor name) 
control-machine name) 
plan-machine name) 
tkm-calculator name) 
foothold-finder name) ) 



contact-sensor : owner self)) 
executor : owner self)) 
control-state-machine : owner 
plan-state-machine : owner 
tkm-calculator : owner self)) 
foothold-finder : owner self)) 



self) ) 
self) ) 



fmethod (contact-confirm leg) 

0 



contact-p contact-sensor) ) 



fmethod (do-planned-motion leg) 

0 

change plan-machine) 
change control-machine) 

move executor (get-Hl owner) (get-inv-Hl owner) 
(get-body-trans-ratel owner) 
(get-body-rotate-ratel owner) ) 
sensing contact-sensor) ) 



fmethod 

get-Hl 



(get-Hl 

0 

owner) ) 



leg) 



fmethod (interlock-confirm leg) 

0 

lay add stable-without-p self 
if (contact-confirm exchanged-leg) 
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t 

nil) ) 



(defmethod (leg-pos-wrt-body leg) 

0 

(leg-pos-wrt-body executor) ) 



(defmethod (lift-able leg) 

0 

(if (equal (state-name plan-machine) ' eligible-to-lift ) 
self 
nil) ) 



(defmethod (lift-ok leg) 

0 

(lift-ok owner name) ) 



(defmethod (lifted leg) 

0 

(lifted owner name) ) 



(defmethod (new-foothold leg) 

0 

(cond ((car foothold-list) 
(set-max self) 
t) 

(t 

nil) ) ) 



(defmethod (permitted-cell leg) 
(t-cell) 

(permitted-cell owner t-cell) ) 



(defmethod (place-able leg) 

0 

(if (equal (state-name plan-machine) 'available-leg) 
self 
nil) ) 



(defmethod (pro jected-pos leg) 

0 

(get-desired-pos executor) ) 



(defmethod (select-foothold leg) 

0 

; out-list: ((max-foothold max-tkm) (foothold-list) 

(let* ( (H (get-H6 owner) ) 

(inv-H (get-inv-H6 owner) ) 



(tkm-list ) ) 
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(setf 
(setf 
(setf 
( setf 
(setf 
(setf 



(body-trans-rate (get-body-trans-ratelO owner) ) 
(body-rotate-rate (get-body-rotate-ratelO owner) ) 
(estimated- support -plane 

(get-estimated-support-plane owner) ) 

(out-list 

(find-foothold foothold-finder 

H inv-H body-trans-rate body-rotate-rate 
estimated-support-plane) ) ) 
foothold (first (first out-list))) 
reserved-foothold foothold) 
tkm (second (first out-list))) 
reserved-tkjn tlcm) 
foothold-list (second out-list) ) 
tlcm-list (third out-list) ) ) ) 



ifmethod (send-decision leg) 

(a-decision) 

(send-decision plan-machine a-decision) ) 



ifmethod (send-decision leg rafter) 
(a-decision) 

(if (equal a-decision 'place) 

(set-desired-pos executor foothold) ) ) 



ifmethod (send-exchange leg) 
(a-leg) 

[setf exchanged-leg a-leg) ) 



ifmethod (set -max leg) 

0 

(do ((footholds (cdr foothold-list) (cdr footholds)) 
(tkms (cdr tkm-list) (cdr tkms) ) 

(max-foothold (car foothold-list) ) 

(max-tkm (car tkm-list)) 

(out-footholds) (out-tkms) ) 

( (null footholds) 

(setf foothold max-foothold) 

(setf tkm max-tkm) 

(setf foothold-list out-footholds) 

(setf tkm-list out-tkms) ) 

(cond ( (> (car tkms) max-tkm) 

(setf max-foothold (car footholds) ) 

(setf max-tkm (car tkms) ) ) 

(t 

(setf out-footholds 

(cons (car footholds) out-footholds) ) 
(setf out-tkms 

(cons (car tkms) out-tkms) ) ) ) ) ) 



ifmethod ( stable-without-p leg) 

0 



(stable-without-p owner self) ) 
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(defmethod (supporting leg) 

0 

(cond ((equal (state-name plan-machine) 'planned-contact) 
self) 

((equal (state-name plan-machine) ' eligible-to-lift ) 
self) 

(t nil) ) 

) 



(defmethod ( supporting-p leg) 

0 

(cond ((equal (state-name control-machine) 'contact) 
self) 

((equal (state-name control-machine) 'support) 
self) 

(t nil)) 

) 



(defmethod (terrain-point leg) 
(t-cell) 

(terrain-point owner t-cell) ) 



(defmethod (TKM-limit leg) 

0 

(cond ( (null tkm) 
self) 

( (< tkm 0 . 1) 
self) 

(t 

nil))) 



(defmethod (TKM-limit-p leg) 

0 

(cond ( (null tkm-p) 
self) 

( (< tkm-p 0.5) 
self) 

(t nil) ) ) 



(defmethod (update-tkm leg) 

0 

(let ((body-trans-rate (get-body-trans-ratelO owner)) 

(body-rotate-rate (get-body-rotate-ratelO owner) ) 
(inv-H (get-inv-HlO owner) ) ) 

(setf tkm (find-tkm tkm-calculator 

(to-body-transf orm inv-H foothold) 
body-trans-rate body-rotate-rate) ) ) 

) 



(defmethod (update-tkm-p leg) 
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0 

(let ( (body-trans-rate-p (get-body-t rans-ratel owner) ) 

(body-rotate-rate-p (get-body-rotate-ratel owner) ) 
(inv-H-p (get-inv-Hl owner) ) ) 

(setf tkm-p (find-tkm tkm-calculator 

(to-body-t ransf orm inv-H-p foothold) 
body-trans-rate-p body-rotate-rate-p) ) ) 

) 



efmethod (with-f oothold leg) 

0 

(cond (reserved-foothold 

(setf foothold reserved-foothold) 

(setf tkm reserved-tkm) 

self) 

(t nil))) 
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^ad-file . lisp 



. Mode: LISP; Package: USER; Syntax: Common-lisp 

lefun load-files () 

(load "ob ject : packdef " ) 

(load "object :math” ) 

(load "object :graph") 

(load "object : display" ) 

(load "object : vision" ) 

(load "ob ject : tkm" ) 

(load "object : foothold") 

(load "object : sensor ") 

(load "object : executor " ) 

(load "object : control-machine") 

(load "object :plan-machine") 

(load "ob ject : leg" ) 

(load "object : stability") 

(load "object : support-plane" ) 

(load "ob ject : h-calculator " ) 

(load "object : command-regulator") 

(load "object : terrain-regulator" ) 

(load "object : body-controller ") 

(load "object :body") 

(load "object : robot ") 

(cp : execute-command "set package" "robot-rules") 
(load "object : robot4") ) 

oad-files) 



I 
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Mode: LISP; Package: ROBOT-MATH; Syntax: Common-lisp 






robot math library 

r*****************************************************»’ 



ifun arc-cos (s) 
[acos s) ) 



sfun atan2 (y x) 

if (> (abs x) 0.000001) ; not zero 

(if (> X 0) 

(atan (/ y x) ) 

(+ (atan (/ y x) ) (* (signum y) PI) ) ) 

(* (signum y) (/ PI 2)))) 



ifun col-mul (mat coll col2) 
let ( (sum 0) ) 

(dotimes (i 4) 

(setf sum (4- sum (* (aref mat i coll) (aref mat i col2) ) ) ) ) 
urn) ) 



fun counting (a-list) 
do ( (a-list a-list (cdr a-list) ) 
(i 0 (+ i 1))) 

( (null a-list) i) ) ) 



ifun crossprod (vectl vect2) 

let* ( (xl (first vectl) ) (x2 (first vect2) ) 

(yl (second vectl) ) (y2 (second vect2) ) 

(zl (third vectl) ) (z2 (third vect2) ) 

(X (- (* yl z2) (* y2 zl) ) ) 

(y (- (* x2 zl) (* xl z2) ) ) 

(z (- (* xl y2) (* x2 yl)))) 

(list X y z) ) ) 



ifun delete-list (a-list b-list) ; delete a-list from b-list 
do ( (deleting-list a-list (cdr deleting-list) ) 

(deleted-list b-list) ) 

( (null deleting-list) deleted-list) 

(setf deleted-list (remove (car deleting-list) 

deleted-list :test 'equal)))) 



ifmacro dequeue (queue) 

(progl (car , queue) 

(setf , queue (cdr ,queue)))) 
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(defun dotprod (vectl vect2) 

; No dimension limitation ! ! ! 

(apply '+ (mapcar vectl vect2) ) ) 



(defmacro enqueue (queue-name element) 

; globals : queue-name 

; Value of recover field of command is a list . 

; Two recover command is possible for one sampling-time. 

; structure of QUEUE : (first second third . . . last) 

' (setq ^queue-name (nconc ^queue-name (list ^element)))) 



(defmacro empty-queue (queue) 
' (setq , queue ' ( ) ) ) 



(defun ident () 

(make-array ' (4 4) : initial-contents 
' ( (1 0 0 0 ) 

(0 10 0 ) 

(0 0 10 ) 

(000 1 )))) 



(defun magnitude (a-vector) 

(sqrt (dotprod a-vector a-vector) ) ) 



(defun magvect (const vect) 

; magvect = const * vect 

(mapcar (lambda (a-element) 

(* const a-element)) 
vect) ) 



(defun matrixadd (mtl mt2) 

(let ( (mt3 (ident) ) ) 

( dot ime s ( i 4 ) 

( dot ime s ( j 4 ) 

(setf (aref mt3 i j) (+ (aref mtl i j) (aref mt2 i j) ) ) ) ) 

mt3)) 



(defun matrixinv (mat ) 

(let ( (px (- (col-mul mat 0 3) ) ) 

(py (~ (col-mul mat 1 3) ) ) 

(pz (- (col-mul mat 2 3) ) ) 

(matrix (transpose mat) ) ) 

(setf (aref matrix 3 0) 0) (setf (aref matrix 3 1) 0) 

(setf (aref matrix 3 2) 0) (setf (aref matrix 3 3) 1) 

(setf (aref matrix 0 3) px) (setf (aref matrix 1 3) py) 

(setf (aref matrix 2 3) pz) 

matrix) ) 
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afun matrixmult (mtl mt2) 

(let ((mat (make-array ' (4 4)))) ; it defines 0 through 3. (4 is not included) 

(dotimes (i 4) ; will repeat i=0^ 1, 2 , and 3. (not 4) 

(dotimes (j 4) 

I (setf (aref mat i j) 0) ; initialize to zero 

(dotimes (k 4) 

(setf (aref mat i j) (+ (aref mat i j) (* (aref mtl i k) 

(aref mt2 k j) ) ) ) ) ) ) 

mat) ) 



sfun nil-list (a-list ) 

(do ( (a-list a-list (cdr a-list) ) 
(not-nil nil) ) 

((null a-list) (not not-nil)) 
(if (car a-list) 

(setf not-nil t) ) ) ) 



afun normalize-vector (a-vector) 
i(let ( (m (magnitude a-vector) ) ) 

(if (< m 0.0000001) 

(list 000) 

(magvect (/ 1.0 m) a-vector)))) 



sfun orthogonalizat ion (mt) 

J ; Gram-Schimit orthogonalization process 

(let* ( (mx (ident) ) 



(tx 


(aref 


mt 


0 


3) ) 


(ty 


(aref 


mt 


1 


3)) 


(t z 


(aref 


mt 


2 


3) ) 


(xl 


(aref 


mt 


0 


0) ) 


(x2 


(aref 


mt 


0 


1)) 


(x3 


(aref 


mt 


0 


2) ) 


(yl 


(aref 


mt 


1 


0) ) 


(y2 


(aref 


mt 


1 


D) 


(y3 


(aref 


mt 


1 


2) ) 


(zl 


(aref 


mt 


2 


0) ) 


(z2 


(aref 


mt 


2 


1) ) 


(z3 


(aref 


mt 


2 


2) ) 



(ml (magnitude (list xl yl zl) ) ) 

(xl (/ xl ml) ) 

(yl (/ yl ml) ) 

(zl (/ zl ml) ) 

(a (dotprod (list xl yl zl) (list x2 y2 z2) ) ) 
(x2 (- x2 (* a xl) ) ) 

(y2 (- y2 (* a yl))) 

(z2 (- z2 (* a zl) ) ) 

(m2 (magnitude (list x2 y2 z2))) 

(x2 (/ x2 m2) ) 

(y2 (/ y2 m2) ) 

(z2 (/ z2 m2) ) ) 



(setf 


(aref 


mx 


0 


0) 


xl) 


(setf 


(aref 


mx 


0 


1) 


x2) 


(setf 


(aref 


mx 


0 


2) 


x3) 


(setf 


(aref 


mx 


1 


0) 


yl) 


(setf 


(aref 


mx 


1 


1) 


y2) 


(setf 


(aref 


mx 


1 


2) 


y3) 


(setf 


(aref 


mx 


2 


0) 


zl) 


(setf 


(aref 


mx 


2 


1) 


Z2) 


(setf 


(aref 


mx 


2 


2) 


z3) 


(setf 
mx) ) 


(aref 


mx 


0 


3) 


tx) 


(setf 


(aref 


mx 


1 


3) 


ty) 


( setf 


(aref 


mx 


2 


3) 


tz) 



efun plane-transform ( plane matrix ) 

Transf ormed-Plane = Plane * Matrix 

plane is defined as ( (a b c) d) . (a b c) is unit normal, d is -(distance) . 
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(let* ( (new-a nil) 

(new-b nil) 

(new-c nil) 

(new-d nil) 

(old-unit-normal (car plane) ) 

(old-d (cadr plane) ) 

(old-a (first old-unit-normal) ) 

(old-b (second old-unit-normal) ) 

(old-c (third old-unit-normal) ) 

(mag nil) ) 



(setf 


new-a 


(+ 


(* 


old-a 


(aref 


matrix 


0 


0)) (* 


old-b 


(aref 


matrix 


1 


0) ) 








(* 


old-c 


(aref 


matrix 


2 


0)))) 












(setf 


new-b 


(+ 


(* 


old-a 


(aref 


matrix 


0 


1) ) (* 


old-b 


(aref 


matrix 


1 


1) ) 








(* 


old-c 


(aref 


matrix 


2 


1) ) ) ) 












(setf 


new-c 


(+ 


(* 


old-a 


(aref 


matrix 


0 


2) ) (* 


old-b 


(aref 


matrix 


1 


2) ) 








(* 


old-c 


(aref 


matrix 


2 


2) ) ) ) 












(setf 


new-d 


(+ 


(* 


old-a 


(aref 


matrix 


0 


3) ) (* 


old-b 


(aref 


matrix 


1 


3)) 








(* 


old-c 


(aref 


matrix 


2 


3) ) old-d) ) 











(setf mag (magnitude (list new-a new-b new-c) ) ) 

(if (< (abs mag) 0.0000001) 

(print "Error in PlaneTransf orm" ) 

(list (list (/ new-a mag) (/ new-b mag) (/ new-c mag) ) 
(/ new-d mag) ) ) ) ) 



(defun plane-distance (plane velocity position) 

; Plane (X - Q)N = 0 , straight line X = P + tA. 

; t = ( Q - P ) N / ( AN ) if A is normalized then t is signed distance. 
; if t is infinitive then plane-distance returnes nil. 

; plane-distance returns t. 

(let* ( (A (normalize-vector velocity) ) 

(N (first plane) ) 

(dis (- (second plane) ) ) 

(Q (magvect dis N) ) ; magvect = const * vector 

(P position) 

(Q_P (vectsub Q P) ) 

(AN (dotprod A N) ) 

(numerator (dotprod Q_P N) ) ) 

(if (< (abs AN) 0.0000001) ; no crossing 

nil ; returns nil 

(/ numerator AN) ) ) ) 



(defun plane-intersection (a-line a-plane) 

; a-line ((direction) (point)) X = P + tA. 

; a-plane ((unit-normal) -dist) (X - Q) N = 0. 

(let* ((velocity (normalize-vector (first a-line))) 

(position (second a-line) ) 

(t-value (plane-distance a-plane velocity position) ) ) 
(if t-value 

(vectadd position (magvect t-value velocity) ) 
nil) ) ) ; no intersection 



(defun plane-normal-distance (a-plane a-point) 
; vector-type-plane (abed) 

; paul-type-point transpose (x y z 1) 



5 
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[(let* ((unit-normal (first a-plane) ) 

(dis (second a-plane) ) 

I (vector-type-plane (reverse (cons dis (reverse unit-normal) ) ) ) 

(paul-type-point (reverse (cons 1 (reverse a-point) ) ) ) ) 
(dotprod vector-type-plane paul-type-point) ) ) 



efun rotatemat (axis angle) ; array index starts from 0 not 1. 
return rotatematrix angle : radian axis : x y or z 
(let ( (mat (ident) ) 

(cosa (cos angle) ) 

(sina (sin angle) ) ) 

(case axis 

(x-axis 



(setf 


(aref 


mat 


1 


1) 


cosa) 


(setf 


(aref 


mat 


1 


2) 


(- sina) ) 


(setf 


(aref 


mat 


2 


1) 


sina) 


(setf 


(aref 


mat 


2 


2) 


cosa) ) 


(y-axis 


(setf 


(aref 


mat 


0 


0) 


cosa) 


(setf 


(aref 


mat 


0 


2) 


sina) 


(setf 


(aref 


mat 


2 


0) 


(- sina) ) 


(setf 


(aref 


mat 


2 


2) 


cosa) ) 


( z-axis 


(setf 


(aref 


mat 


0 


0) 


cosa) 


(setf 


(aref 


mat 


0 


1) 


(- sina) ) 


(set f 


(aref 


mat 


1 


0) 


sina) (setf (aref mat 


. 1 


1) 


cosa) ) ) 



mat)) / returns this value. 



ijefun to-body-transform (inv-H point s-wrt-earth) 

^returns points-wrt-body 

(if (listp (first point s-wrt-earth) ) ; test multi-points 

(do ( (points point s-wrt-earth (cdr points) ) ; multi-points case 

(out-points nil) ) 

( (null points) (reverse out-points) ) 

(setf out-points (cons (transform inv-H (car points) ) out-points) ) ) 
(transform inv-H points-wrt-earth) ) ) ; single point case 



jefun to-earth-t ransf orm (H points-wrt-body) 

jreturns points-wrt-earth 

i(if (listp (first points-wrt-body)) 

(do ( (points points-wrt-body (cdr points) ) 

I (out-points nil) ) 

' ( (null points) (reverse out-points) ) 

I (setf out-points (cons (transform H (car 

I (transform H points-wrt-body) ) ) 



; test multi-points 
; multi-points case 



points) ) out-points) ) ) 
; single point case 



Jefun transform (mat point) ; array index starts from 0 not 1. 
(let ( (x (car point)) 

(y (cadr point) ) 

(z (if (caddr point) (caddr point) 0) ) ) 



(list (+ 


(* X (aref mat 0 0) ) 
(aref mat 0 3) ) 


(* 


y 


(aref 


mat 


0 


1) ) 


( * Z 


(aref 


mat 


0 


2) ) 


(+ 


(* X (aref mat 1 0) ) 
(aref mat 1 3) ) 


(* 


y 


(aref 


mat 


1 


D) 


( * z 


(aref 


mat 


1 


2) ) 


( + 


(* X (aref mat 2 0) ) 
(aref mat 2 3 ) ) ) ) ) 


(* 


y 


(aref 


mat 


2 


1) ) 


( * z 


(aref 


mat 


2 


2) ) 



efun transmat (x y z) 
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; returns translational marix 
(let ( (matrix (ident) ) ) 

(setf (aref matrix 0 3) x) 
(setf (aref matrix 1 3) y) 
(setf (aref matrix 2 3) z) 
matrix) ) 



(defun transpose (mat) 

(let ( (matrix (make-array ' {4 4) ) ) ) 

(dotimes (i 4) 

(dotimes (j 4) 

(setf (aref matrix i j) (aref mat j i) ) ) ) 
matrix) ) 



(defun unit-crossprod (vectl vect2) 

; generate unitnormal vector of vectl X vect2 



(let* ( (xl 


(first 


vectl) ) (x2 (first 


vect2) ) 


(yi 


(second 


vectl)) (y2 (second 


vect2) ) 


(Zl 


(third 


vectl)) (z2 (third 


vect2) ) 


(X 


(- (* yl 


z2) 


(* y2 zl))) 




(y 


(- (* x2 


zl) 


(* xl z2) ) ) 




(z 


(- (* xl 


y2) 


(* x2 yl))) 




(m 


(sqrt (+ 


( * X 


x) (* y y) (* 


z z))))) 


(list (/ 


X m) (/ 


y m) 


(/ z m) ) ) ) 





(defun vectadd (vectl vect2) 
; vectsub = vectl + vect2 
; no limit in dimension 
(mapcar '+ vectl vect2) ) 



(defun vectsub (vectl vect2) 
; vectsub = vectl - vect2 
; no limit in dimension 
(mapcar vectl vect2) ) 
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. Mode: LISP; Syntax: Common- lisp; Package: USER 

•efpackage robot-common 
I ( : use) 

( : export 

legl leg2 leg3 leg4 leg5 leg6 
I lift place exchange) ) 



[efpackage robot-math 
! ( : use symbolics -common- lisp) 
i ( : export 
vectsub 
vectadd 

unit-crossprod 

transpose 

transmat 

transform 

to-earth-t ransf orm 
to-body-transf orm 
rotatemat 

plane -normal -distance 
I plane-intersection 
i plane-distance 
plane-t ransf orm 
orthogonalizat ion 
normalize -vector 
nil-list 
matrixmult 
matrixinv 
mat rixadd 
magvect 
magnitude 
ident 

empty-queue 

enqueue 

dotprod 

dequeue 

delete-list 

crossprod 

counting 

col -mu 1 

atan2 

arc-cos 

x-axis y-axis z-axis) ) 



lefpackage terrain 

(:use robot -math symbolics-common-lisp) 

( : export 

init-terrain 
graph-terrain 
display modify 
create 

permitted-cell get-height in-side-of-whole-terrain terrain-point 
graph-asv 
init-data 
display 
joystick 
reset 

get- joy-value) ) 



lefpackage robot 
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(ruse robot -’Common robot -math symbolics -common- lisp pl-user) ) 



(defpackage leg 

(ruse robot-common robot -math terrain robot symbolics-common-lisp) 

( : export 
leg 
init 

leg-naime 
leg- foot hold 
leg-tkm 
send-exchange 
do-planned-mot ion 
simulation-output 
leg-pos-wrt-body 
lift-able 
place-able 
supporting 
support ing-p 
send-decision 
select -foot hold 
update-tkm 
update -t km- p 
new-foothold 
t km- limit 
with- foot hold 
tkm-limit-p 
) ) 



(defpackage body 

(ruse robot-common robot-math leg symbolics-common-lisp) 

( r export 
body 
init 
get-Hl 
get-inv-Hl 
get-H6 
get-inv-H6 
get-inv-HlO 
get-body-trans-rat el 
get-body-rotate -rate 1 
get-body- trans-ratelO 
get-body-rotate- rat el 0 
get-estimated- support -plane 
stable 

calculate -mot ion 
more-stable 
slow-down 
speed-up 

near-dead-lock-test 

stable-p 

stable-p-m 

stable-m 

modify- command 

modify-command-p 

restore-command 

restore-command-p 

stop-motion 

restore-motion 
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lefpackage vision 

(;use robot-coiranon robot -math leg robot terrain symbolics-common-lisp) 
( : export 

' vision-system 
init 

' scanning 

permit ted-cell 
terrain-point) ) 



efpackage robot 

(:use robot-common robot -math terrain leg body vision symbolics-common-lisp pl-user) 
I ( : export 

creat e-terrain 

kill-terrain 

inits 

graphical^di splay 
execute_planned_motion 
send_decision 
stable_p 
max_sm__leg 
stable_p_m 
modify_command 
stable_without 
sma 1 1 e s t _t km_l e g 
updat e_robot_s t a t u s 
read_ joy stick 
restore_command 
slow_down_robot 
speed_up_robot 
j leg_with_new^f oothold 
I at_tkm_limit 
do^recovery 
has_more_tkm 
get-Hl 
get-inv-Hl 
get-H6 
get-inv-H6 
get-inv-HlO 
get-body-trans-ratel 
get -body-rot ate -rat el 
get-body-trans-ratelO 
get -body- rot ate -rat el 0 
get -estimated- support -plane 
lift-ok 
lifted 

stable-without-p 

terrain-point 

)) 



lefpackage robot -rules 

( : use prolog-global robot-common robot symbolics -common- lisp )) 




n if :i / r, 
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I 

Mode; LISP; Syntax: Common-lisp; Package: LEG 
I plan-state flavor definition 



iefflavor plan-state ((decision nil) (observation nil) 

(condition nil) ) 



( state) 

: init able -instance -variables) 



(command nil) 



iefmethod (gene rate- command plan-state) 

0 

command) 



iefmethod (change plan-state) 

(given-decision observed-state given-condition) 
(cond ( (and decision (listp decision) ) 

(cond ( (equal given-decision (first decision) ) 
(first next-state) ) 

( (equal given-decision (second decision) ) 
(second next-state) ) 

(t self))) 

(condition 

(if (and (equal given-condition condition) 

(equal observed-state observation) ) 
next-state 
self) ) 

(t 

(cond ( (equal observed-state observation) 
next-state) 

( (equal given-decision decision) 
next-state) 

(t self))))) 



k'k'k'k'k'k'k'k’k'k:k'k'k'k'k9('k'^'^'^-A"k’k’k'k'^'k9('k'k'k'k'k'k’k'k'k'k’k'k'^'h'k'k'^’k’k'k'k'f('k'k'A'k'k'k 

plan-state-machinie flavor definition 



iefflavor plan-state-machine ((decision nil) (observation nil) 

(condition nil) (lift-ready-flag nil) 
control-ma chine) 

(state-machine) 

: initable-instance-variables) 



iefmethod (init plan-state-machine) 

(leg-name) 

(if (member leg-name ' (legl leg4 leg5) ) 

(init-plan-machine self ' eligible-to-lift ) 
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(init-plan-machine self 'available-leg)) 

(setf control-machine (leg-control-machine owner))) 



(defmethod (init-plan-machine plan-state-machine) 

(a- St at e-name) 

(let (available-leg planned-contact eligible-to-lift 
planned-lift actual-lift planned-exchange) 

(setf actual-lift 

(make-instance 'plan-state 

; name 'actual-lift 
: observation 'ready 
: command ' re cover- command ) ) 

(setf planned-lift 

(make-instance 'plan-state 

: name 'planned-lift : condition ' stable-without 
: observation 'support 
:next-state actual-lift)) 

(setf planned-exchange 

(make-instance 'plan-state 

:name 'planned-exchange : condition 'interlock-confirm 
: observation 'support 
:next-state actual-lift)) 

(setf eligible-to-lift 

(make-instance 'plan-state 

: name 'eligible-to-lift 
: decision ' (lift exchange) 

: next-state (list planned-lift planned-exchange))) 

(setf planned-contact 

(make-instance 'plan-state 

: name 'planned-contact .'observation 'contact 
: command ' deploy-command 
: next-state eligible-to-lift)) 

(setf available-leg 

(make-instance 'plan-state 

:name 'available-leg :decision 'place 
: next-state planned-contact)) 

(set-next-state actual-lift available-leg) 



) 



(setf state (cond ( (equal a-state-name 

available- leg) 

( (equal a-state-name 
planned-contact) 

( (equal a-state-name 
eligible-to-lift ) 

( (equal a-state-name 
planned-lift) 

( (equal a-state-name 
planned-exchange) 

( (equal a-state-name 
actual-lift) ) ) 

) 



(state-name 
(state-name 
(state-name 
(state -name 
(state-name 
(state-name 



available-leg) ) 
planned-contact) ) 
eligible-to-lift) ) 
planned-lift) ) 
planned-exchange) ) 
actual-lift) ) 



(defmethod (change plan- state-machine : before) 

0 

(setf observation (state-name control-machine) ) 

(cond ((and (equal (state-name state) 'planned-exchange) 
(interlock-confirm owner) 

(stable-without-p owner) 

(lift-ok owner) ) 



I an-machine . lisp 



Wed Mar 30 15:28:06 1988 



3 



(setf lift-ready-flag t) 

(setf condition 'interlock-confirm)) 

((and (equal (state-name state) 'planned-lift) 
(stable-without-p owner) 

(lift-ok owner) ) 

(setf lift-ready-flag t) 

(setf condition ' stable-without ) ) 

(t 

(setf condition nil) 

(setf lift-ready-flag nil) ) ) 



^efmethod (change plan-state-machine) 

0 

(setf state (change state decision observation condition) ) ) 



iefmethod (change plan-state-machine rafter) 

0 

(send-command control-machine 

(generate-command state) ) 

, (if (and lift-ready-flag 

(equal (state-name self) 'actual-lift)) 
(lifted owner) ) ) 



lefmethod (send-decision plan-state-machine) 
(a-decision) 

(setf decision a-decision) ) 
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. Mode: LISP; Package: ROBOT; Syntax: Common-lisp; Base: 10 

lefvar asv) 






robot flavor definition 

iefflavor robot (legs body vision-system joystick 

(lift-able-legs nil) 

(place-able-legs nil) (supporting-legs nil) 
(supporting-p-legs nil) 

(joy-command ' (0 0 0)) lift-queue lift-flag) 

0 

: initable-instance-variables) 



letf asv (make-instance 'robot)) 



iefmethod (init robot) 

0 

(init-data graph-asv) 

(empty-queue lift-queue) 

(setf lift-flag t) 

(setf joystick (make-instance ' joystick) ) 

(reset joystick) 

(setf vision-system (make-instance 'vision-system :owner self)) 
(init vision-system) 

(let ((H)) 

(setf body (make-instance 'body : owner self)) 

(setf H (init body) ) 

(setf legs (list 



(make- instance 


' leg 


: name 


'legl 


: owner 


self) 


(make-instance 


'leg 


: name 


'leg2 


: owner 


self) 


(make-instance 


'leg 


: name 


'legs 


: owner 


self) 


(make- instance 


' leg 


: name 


'leg4 


: owner 


self) 


(make-instance 


'leg 


: name 


' leg5 


: owner 


self) 


(make -instance 


'leg 


: name 


' leg6 


: owner 


self) 



)) 

(mapcar (lambda (a-leg) (init a-leg H) ) legs) ) 

) 



Iefmethod ( f ind-lift-able-legs robot) 

0 

(delete nil (mapcar 'lift-able legs))) 



Iefmethod (f ind-place-able-legs robot) 

0 

(delete nil (mapcar 'place-able legs))) 



iefmethod (find-supporting-legs robot) 

0 
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(delete nil (mapcar 'supporting legs))) 



(defmethod (f ind-supporting-p-legs robot) 

0 

(delete nil (mapcar ' supporting-p legs))) 



(defmethod (get-body-rotate-ratel robot) 

0 

(get-body-rotate-ratel body) ) 



(defmethod (get-body-rotate-ratelO robot) 

0 

(get-body-rotate-ratelO body) ) 



(defmethod (get-body-trans-ratel robot) 

0 

(get-body-trans-ratel body) ) 



(defmethod (get-body-trans-ratelO robot) 

0 

(get-body-trans-ratelO body) ) 



(defmethod (get-estimated-support-plane robot) 

0 

(get-estimated-support-plane body) ) 



(defmethod (get-Hl robot) 

0 

(get-Hl body) ) 



(defmethod (get-H6 robot) 

0 

(get-H6 body) ) 



(defmethod (get-inv-Hl robot) 

0 

(get-inv-Hl body) ) 



(defmethod (get-inv-H6 robot) 

0 

(get-inv-H6 body) ) 



3 
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iefmethod (get-inv-HlO robot) 

0 

(get-inv-HIO body) ) 



Iefmethod (lift-ok robot) 

(leg-name) 

(cond (lift-flag 

(cond ( (equal leg-name (leg-name (first lift-queue) ) ) 
(setf lift-flag nil) 
t) 

(t 

nil) ) ) 

(t nil) ) ) 



iefmethod (lifted robot) 

(leg-name) 

(if (equal leg-name (leg-name (first lift-queue) ) ) 
(dequeue lift-queue) 

(print (list "error in lifting" leg-name) ) ) ) 



iefmethod (permitted-cell robot) 
(t-cell) 

(permitted-cell vision-system t-cell) ) 



iefmethod (scanning robot) 

0 



(scanning vision-system) ) 



defmethod (stable-without-p robot) 

(a-leg) 

(stable-p body 

(remove a-leg support ing-p-legs) ) ) 



iefmethod (terrain-point robot) 
(t-cell) 

(terrain-point vision-system t-cell) ) 






terrain interface functions 



lefun create-terrain () 
(init-terrain) ) 
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(defun kill-terrain ( ) 

(zl-user : kill-all-windows) ) 



.******************************************************** 

t 

r 

; Prolog interface robot methods 

f 

f 



(defmethod (at-tkm-limit robot) 

0 



(let ( (limit-leg 

(car (delete nil 

(mapcar 'TKM-limit lift-able-legs) ) ) ) ) 
(setf supporting-legs (remove limit-leg 

supporting-legs) ) 

(setf lift-able-legs (remove limit-leg 

lift-able-legs) ) 

limit-leg) ) 



(defmethod ( check- stability-p robot) 

0 

(stable-p-m body supporting-p-legs (first lift-queue))) 



(defmethod (check-tkm-limit-p robot) 

0 

(delete nil 

(mapcar ' TKM-limit-p supporting-p-legs))) 



(defmethod (do-recovery robot) 

0 

(car 

(delete nil 

(mapcar ' with-foothold place-able-legs) ) ) ) 



(defmethod (execute-planned-motion robot) 

0 

(mapcar 'do-planned-motion legs)) 



(defmethod (graphical-display robot) 

0 

(display graph-asv (get-Hl body) 

(mapcar ' leg-pos-wrt-body legs)) 



) 
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iefmethod (has-more-tkm robot) 
(legl leg2) 

(> (leg-tkm legl) 

(leg-t)cin leg2) ) ) 



iefmethod (leg-with-new-foothold robot) 

0 

return a-leg with new-f oothold , 

(do ( (new-foothold-flags (mapcar ' new-f oothold place-able-legs) 

(mapcar ' new-f oothold place-able-legs)) 

(a-leg nil) ) 

( (or (nil-list new-foothold-flags) 
a-leg) 

(if a-leg a-leg nil) ) 

(setf a-leg (max-sm-leg self nil) ) ) ) 



iefmethod (max-sm-leg robot) 

(a-leg) 

(if place-able-legs 

(do ( (legs (cdr place-able-legs) (cdr legs) ) 

(largest-leg (car place-able-legs) largest-leg) 
(temp-support-legs (remove a-leg supporting-legs) ) ) 

( (null legs) 

(if (and 

(leg-foothold largest-leg) 

(stable body (cons largest-leg temp-support-legs) ) ) 
largest-leg 
nil) ) 

(if (more-stable body temp-support-legs 
(car legs) largest-leg) 

(setf largest-leg (car legs) ) ) ) 

nil) ) 



Iefmethod (modify- command robot) 

0 

(modify-command body) ) 



jdefmethod (wait-for-lift robot) 

! 

i (delete nil 

(mapcar ' lif t-not-done supporting-p-legs) ) ) 



defmethod (read- joystick robot) 

0 

(let ((joy-value (get- joy-value joystick))) 

(if (fourth joy-value) 

(modify graph-terrain) ) 

(setf joy-command 

(reverse (cdr (reverse joy-value) ) ) ) ) ) 



defmethod (re store -command robot) 

0 
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(restore-command body) ) 



(defmethod (send-decision robot) 

(legl leg2 a-decision) 

(cond ((equal a-decision 'exchange) 
(enqueue lift-queue legl) ) 
((equal a-decision 'lift) 
(enqueue lift-queue legl) ) ) 
(cond ((equal a-decision 'exchange) 

(send-decision legl a-decision) 



(send-decision 
(s end-exchange 
(t 

(send-decision 



leg2 

legl 



' place) 
leg2) ) 



legl a-decision) ) ) ) 



(defmethod ( smallest-t)cm-leg robot) 

0 

; select smallest-TKM-leg 
; tkm is nil or positive 

(do ( (legs (cdr lift-able-legs) (cdr legs) ) 

( smallest-leg (car lift-able-legs) ) 

(smallest-tkm nil) (tkm nil) ) 

( (null legs) smallest-leg) 

(setf smallest-tkm (if (leg-tkm smallest-leg) 

(leg-tkm smallest-leg) -1000) ) 

(setf tkm (if (leg-tkm (car legs) ) 

(leg-tkm (car legs) ) -1000) ) 

(if (> smallest-tkm tkm) (setf smallest-leg (car legs) ) ) 

(if (and (equal smallest-tkm -1000) (equal tkm -1000) ) 

"Error : more than one legs are out of kinematic limit") ) ) 



(defmethod (slow-down-robot robot) 

0 

(slow-down body) ) 



(defmethod (speed-up-robot robot) 

0 

(speed-up body) ) 



(defmethod (stable robot) 

0 

(stable body supporting-legs) ) 



(defmethod (stable_m robot) 

0 

(stable-m body supporting-legs) ) 



(defmethod (stable-without robot) 

(a-leg) 

(stable body (remove a-leg supporting-legs) ) ) 
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iefmethod (update-robot-status robot) 

0 

(scanning self) 

(setf lift-flag t) 

(setf lif t-able-legs ( f ind-lift-able-legs self) ) 

(setf place-able-legs (f ind-place-able-legs self) ) 
(setf supporting-legs (find-supporting-legs self) ) 
(setf support ing-p-legs (find-support ing-p-legs self) ) 
(mapcar ' update-t )cin-p supporting-p-legs ) 

(if (check-tkm-limit-p self) 

(stop-motion body (check-tkm-limit-p self) ) 
(restore-motion body) ) 

(if (not (check-stability-p self) ) 

(modify-command-p body) 

(restore-command-p body) ) 

(calculate-mot ion body joy-command legs) 

(mapcar 'select-foothold place-able-legs) 

(mapcar 'update-tkm supporting-legs)) 



Prolog interface functions 

******★★★★***★****★*★******★★****★*★★***★★********★***** 



defun at__tkm_limit ( ) 
(at-tkm-limit asv) ) 



defun do^recovery ( ) 
(do-recovery asv) ) 



defun execute__planned_motion ( ) 
(execute-planned-motion asv) ) 



defun graphical^display ( ) 
(graphical-display asv) ) 



defun has_jnore_tkm (legl leg2) 
(has-more-tkm asv legl leg2) ) 
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(defun init s ( ) 
(init asv) ) 



(defun leg_with_new_f oothold ( ) 
(leg-with-new-f oothold asv) ) 



(defun max_sm_leg (a-leg) 
(max-sm-leg asv a-leg) ) 



(defun modify_coiranand ( ) 
(modify-coiranand asv) ) 



(defun read_joystick ( ) 
(read- joystick asv) ) 



(defun restore^command ( ) 
(re St ore -command asv) ) 



(defun send^decision (legl leg2 a-decision) 

( send-decision asv legl leg2 a-decision) ) 



(defun smallest_tkm_leg ( ) 
(smallest-tkm-leg asv) ) 



(defun slow_down_robot ( ) 
(slow-down-robot asv) ) 



(defun speed_up_robot () 
(speed-up-robot asv) ) 



(defun stable_p() 
(stable asv) ) 



(defun stable_p_m() 
(stable m asv) ) 



(defun stable^without (a-leg) 
(stable-without asv a-leg) ) 
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lefun update_robot_st atus ( ) 
(update-robot-status asv) ) 
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.. Mode: LISP; Syntax: Common- lisp; Package: LEG 

sensor flavor definition 

lef flavor sensor (state owner) 

0 

: initable-instance-variables) 

contact-sensor flavor definition 

ief flavor contact-sensor ( ) 

(sensor) 

; initable-instance-variables) 



defmethod (init contact-sensor) 
(leg-name) 

(setf state (sensing self) ) ) 

I 

defmethod (contact-p sensor) 

0 

state) 



defmethod (sensing sensor) 

0 

simulation purpose 
(setf state 

(let* ( (leg-pos-wrt-body (leg-pos-wrt-body 

(leg-executor owner) ) ) 

(leg-pos-wrt -earth 

(to-earth-transf orm (get-Hl owner) leg-pos-wrt-body) ) 
(x-y-pos (list (first leg-pos-wrt-earth) 

(second leg-pos-wrt-earth) ) ) 

(leg-height (third leg-pos-wrt-earth) ) ) 

(if (< leg-height (+ 0.07 (third 

(terrain-point owner x-y-pos) ) ) ) 



t 

nil)))) 






“«ni 
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. Mode: LISP; Syntax: Common-lisp; Package: BODY 



t-A-A’k’k-k-k'A-A-k’k'k'k-k-k'k'k'k'k'k-A'k'k-k'k-k'k’k'k-k’k'k-k'k’k'k'k'k'k'k'k'k'k'k'k-k'k'k'k-k'k'k-k-k'k'k-k 

Stability-calculator flavor definition 

fk-kic-k-^'kic'kic-A'k'k'k-A-k-k'k-k'k-k-kicit-k-k’k'k'k'k-k'k-kic-k-k'kic'k'k'k'kic^'Aificit-kit'kifit-AitifiK 

ief flavor stability-calculator (convex-hull-order 

safety-margin 

safety-margin-p 

large- safety-margin 

large -safety-margin-p 

recovery-vector 

recovery-vector-p 

owner) 

I 0 

: init able- instance- variables) 



iefmethod (init stability-calculator) 

0 

(setf convex-hull-order ' (leg2 leg4 leg6 leg5 leg3 legl) ) 
(setf safety-margin 0.4) 

(setf safety-margin-p 0.2) 

(setf large-safety-margin 0.5) 

(setf large-saf ety-margin-p 0.4) 

(setf recovery-vector ' (0 0 0) ) 

(setf recovery-vector-p ' (0 0 0) ) ) 



iefmethod (get-recovery-vector stability-calculator) 

O 

recovery-vector ) 



sfmethod (get-recovery-vector-p stability-calculator) 



0 

recovery-vector-p) 



iefmethod (convert-to-recovery-vector stability-calculator) 
(stability-vector) 

(let ( (sm (first stability-vector) ) 

(vect (second stability-vector) ) ) 

(cond ( (< sm 0) 
nil) 

( (< sm 0.1) 

(magvect (/ 1 sm) vect)) 

(t 

(magvect (/ 0.1 (* sm sm) ) vect))))) 



iefmethod (more-stable stability-calculator) 
(supporting-legs H legl leg2) 

(let ( (stabilityl (calculate-st ability self 

(cons 

(stability2 (calculate-stability self 

(cons 



(if (> stabilityl stability2) 



t 



legl 

leg2 



support ing- legs) 
support ing-legs) 



H) ) 
H))) 
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nil))) 



(defmethod (stable-m stability-calculator) 

; predict H <= HlO 

(supporting-legs H) 

(let ( (stability-vector 

(get-stability self supporting-legs H) ) ) 

(cond ( (>= (first stability-vector) 
large- safety-margin) 
t) 

(t 

(if (>= (first stability-vector) safety-margin) 

(setf recovery-vector 

(convert-to-recovery-vector self stability-vector) ) 
(setf recovery-vector ' (0 0 0))) 
nil)))) 



(defmethod (stable-p-m stability-calculator) 

; present H <= HI 

( supporting-p-legs H) 

(lef^ ( (stability-vector 

(get-stability self supporting-p-legs H) ) 

(st-margin (first stability-vector))) 

(cond ( (>= st-margin 

large -safety-margin-p) 
t) 

(t 

(setf recovery-vector-p 

(convert-to-recovery-vector self stability-vector) ) 
; (if (< st-margin safety-margin-p) 

; (print (list ' st-p st-margin))) 

nil)))) 



(defmethod (stable stability-calculator) 
(supporting-legs HlO) 

(if (>= (calculate-stability self 

supporting-legs 

safety-margin) 



t 



nil) ) 



HlO) 



(defmethod (stable-p stability-calculator) 
(supporting-p-legs HI) 

(if (>= (calculate-stability self 

supporting-p-legs 

safety-margin-p) 

t 

nil)) 



HI) 



(defmethod (calculate-stability stability-calculator) 
(supporting-legs H) 

(first (get-stability self supporting-legs H) ) ) 
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afmethod (get-stability stability-calculator) 

(supporting-legs H) 

(if (>= (counting supporting-legs) 3) 

(measure-distance ( cent er-of -gravity H) 

(convex-hull-point s 
(supporting-points 

(find-order self supporting-legs) ) ) ) 

' (- 100.0 (0 0 0 ) ) ) ) 



efun center-of-gravity (H) 

center-of-body is represented wrt earth coordinate, 
(let ( (x (aref H 0 3) ) 

(y (aref H 1 3) ) ) 

(list X y) ) ) 

2 enter-of-body can be changed in future. 



sfun convex-hull-points (points) 

point is a list (x y z) . For a time being, only x,y are used. 

nath lib : delete-list 

(if (> (counting points) 3) 

(let* ( (boundary-points (out-side points) ) 

(remaind (delete-list boundary-points points) ) ) 
(cond (remaind (convex-hull-points boundary-points) ) 

(T boundary-points) ) ) 

points)) ; minimum points (3) are reached. 



sfmethod (find-order stability-calculator) 

(legs) 

jlobals : leg-name 

find ordered leg-names for calculating convex-hull-points 
::onvex-hull-order (This has only ready position for leg names) 
(let* ( (ordered-legs nil) ) 

(empty-queue ordered-legs) 

(dolist (a-leg-name convex-hull-order) 

(dolist (a-leg legs) 

(if (equal a-leg-name (leg-name a-leg) ) 

(cons a-leg ordered-legs) ) ) ) 

(reverse ordered-legs) ) ) 

(enqueue ordered-legs a-leg) ) ) ) 
ordered-legs) ) 



2 fun find-slope (first-point second-point) 

(let ( (del-x (- (car second-point) (car first-point))) 
(del-y (- (cadr second-point) (cadr first-point)))) 
(if (> (abs del-x) 0.0000001) 

(/ del-y del-x) 
nil) ) ) 



sfun infinite-case (x a-line) 

(list X 

(+ (* (car a-line) x) (cadr a-line) ) ) ) 
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(defun intersection-point (a-line b-line) 

; Returns list (x y) . Line is list (slope crossing-point-of-axis) . 

(cond ( (null (car a-line) ) (infinite-case (cadr a-line) b-line) ) 

( (null (car b-line) ) (infinite-case (cadr b-line) a-line) ) 

(t (normal-case a-line b-line) ) ) ) 



(defun in-side-of-convex-hull (center-point first-points second-points) 

(do* ( (first-points first-points (cdr first-points) ) 

(second-points second-points (cdr second-points) ) 

(in-side-flag T) ) 

( (null first-points) in-side-flag) 

(if (test-out-side (car first-points) center-point (car second-points) ) 
(setf in-side-flag nil) ) ) ) 



(defun line (slope point) 

(if slope 

(list slope (- (second point) (* slope (first point) ) ) ) 

(list slope (first point) ) ) ) 

; When slope is infinitive, return with x-axis crossing point instead of 
; y-axis crossing point . 



(defun measure-distance (center-point convex-points) 

; convex-points is a list of points 
; point is a list (x y z) . 

(let* ( (first-points convex-points) 

(second-points (append (cdr convex-points) 

(list (car first-points) ) ) ) ) 

(if (in-side-of-convex-hull center-point first-points second-points) 
(start-measure center-point first-points second-points) 

' (- 10.0 (0 0 0 ) ) ) ) ) 

; center-of-gravity is out-side of support pattern 



(defun normal-case (a-line b-line) 
(let* ( (al (car a-line) ) 

(bl (cadr a-line) ) 

(a2 (car b-line) ) 

(b2 (cadr b-line) ) 

(X (/ (- bl b2) (- a2 al) ) ) 
(y (+ (* al X) bl) ) ) 

(list X y) ) ) 



(defun out-side (points) 

; this function does not change the order of points except deletion. 

(do* ((first-points points (cdr first-points)) 

(second-points (reverse (cons (car points) (reverse (cdr points) ) ) ) 
(cdr second-points) ) 

(third-points (reverse (cons (car second-points) 

(reverse (cdr second-points) ) ) ) 

(cdr third-points) ) 
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(out-points nil out-points) ) 

( (null first-points) 

(let ( (return-points nil) ) 

(empty-queue return-points) 

(dolist (a-point points) 

(if (member a-point out-points) 

(enqueue return-points a-point) ) ) 
return-points) ) 

(if (test-out-side (car first-points) (car second-points) 

(car third-points) ) 

(setf out-points (cons (car second-points) out-points) ) ) ) ) 



3 fun point-distance (center-point first-point second-point) 
returns distance and vector between cross-pt and center-pt 
(let* ( (slopel (find-slope first-point second-point) ) 

(slope2 (right-angle slopel) ) 

(cross-pt (intersection-point (line slopel first-point) 

(line slope2 center-point))) 
(vectsub center-point cross-pt ) ) 

(magnitude del-vect) ) ) 

(list (first del-vect) (second del-vect) 0.0)))) 



(del-vect 
(distance 
(list distance 



2 fun right-angle (slope) 

(cond ((null slope) 0.0) 

( (< (abs slope) 0.0000001) 
(t (/ (- 1) slope)))) 



; infinitive input slope 
nil) ; zerop slope 



2 fun start-measure (center-point first-points second-points) 

(do* ( (first-points first-points (cdr first-points) ) 

(second-points second-points (cdr second-points) ) 

(min-distance 10000.0 min-distance) ; infinte dummy number 10000.0 
(min-direction nil) (dis-dir nil) ) 

( (null first-points) (list min-distance min-direction) ) 

(setf dis-dir (point-distance center-point 

(car first-points) (car second-points) ) ) 
(cond ( (< (first dis-dir) min-distance) 

(setf min-distance (first dis-dir) ) 

(setf min-direction (second dis-dir) ) ) ) ) ) 



2 fun supporting-points (legs) 

(let ( (points) ) 

(empty-queue points) 

(do ( (legs legs (cdr legs) ) 

(a-pos nil) ) 

( (null legs) 
points) 

(setf a-pos (leg-foothold (car legs) ) ) 
(if a-pos 

(enqueue points a-pos) ) ) ) ) 



sfun test-out-side (first-point second-point third-point) 

(let* ( (a (- (cadr first-point) (cadr third-point) ) ) 

(b (- (car third-point) (car first-point) ) ) 

(c (- (+ (* a (car third-point) ) (* b (cadr third-point) ) ) ) ) 
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(decision (+ (* a (car second-point) ) 
b (cadr second-point)) 
c))) 

(if (>= decision 0.0) 

T 

nil))) 
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. Mode: LISP; Syntax: Common-lisp; Package: BODY 

support-plane-estimator flavor definition 

ief flavor support-plane-estimator (owner) 

0 

) 



iefmethod 

) 



(init support-plane-estimator) 

0 



Iefmethod (get-plane support-plane-estimator) 

(legs) 

(let* ( ( f ootholds-for-estimation (get-footholds legs) ) 

(constants (get-constants f ootholds-for-estimation) ) ) 
(make-plane-f rom-coef f icient constants) ) ) 






support-plane-estimator . get-plane 



lefun add-points (points) 

returns a list (number-of-point s sum-of-points) . 
(do ((points points (cdr points)) 

(i 0 (+ i D) 

(sum-vect ' (0 0 0) ) ) 

( (null points) (list i sum-vect) ) 

(setf sum-vect (vectadd (car points) sum-vect)))) 



lefun average-point (points) 

(let* ( (num-&- sum-vect (add-points points) ) 

(number-of-point s (first n\im-&-sirm-vect ) ) 

(sirm-vect (second num-&- sum-vect) ) ) 

(if (> number-of-point s 0) 

(magvect (/ 1 number-of-point s) sum-vect) 

(print "Error in finding average-point of estimate plane") ) ) ) 



iefun get-aO (bar-point al) 

(let* ( (x-bar (first bar-point) ) 
(z-bar (third bar-point))) 
(- Z-bar (* al x-bar) ) ) ) 
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(defun get-al (points bar-point common-denominator) 

/ returns al which is sum in this function. 

(do* ( (points points (cdr points) ) 

(sum 0) 

(x nil) (x-bar (first bar-point) ) 

(z nil) (z-bar (third bar-point) ) ) 

( (null points) (/ sum common-de nominator) ) 
(setf X (first (car points) ) ) 

(setf z (third (car points))) 

(setf sum (+ sum (* (- x x-bar) (- z z-bar) ) ) ) ) ) 



(defun get-a2 (points bar-point common-denominator) 

; returns a2 which is sum in this function. 

(do* ( (points points (cdr points) ) 

( sum 0) 

(x nil) (x-bar (first bar-point) ) 

(y nil) (y-bar (second bar-point) ) ) 

( (null points) (/ sum common-denominator) ) 
(setf X (first (car points) ) ) 

(setf y (second (car points) ) ) 

(setf sum (+ sum (* (- x x-bar) (- y y-bar)))))) 



(defun get-a3 (bar-point a2) 

(let* ( (x-bar (first bar-point) ) 
(y-bar (second bar-point) ) ) 
(- y-bar (* a2 x-bar) ) ) ) 



(defun get-a4 (points aO al a2 a3) 

(let* ( (number-of-points (counting points) ) 

(yr (get-yr points a2 a3) ) 

(zr (get-zr points aO al) ) 

(yr-bar (get-yr-bar yr n\omber-of-points) ) 

(zr-bar (get-zr-bar zr number-of-points) ) ) 

(do ( (yr yr (cdr yr) ) 

(zr zr (cdr zr) ) 

(numerator 0) (a-yr 0) (a-zr 0) 

(denominator 0) ) 

( (null yr) (/ numerator denominator) ) 

(setf a-yr (first yr) ) 

(setf a-zr (first zr) ) 

(setf numerator (+ numerator (* (- a-yr yr-bar) (- a-zr zr-bar) ) ) ) 
(setf denominator (+ denominator (* (- a-yr yr-bar) (- a-yr yr-bar))))))) 



(defun get-common-denominator (points bar-point) 

(do* ( (points points (cdr points) ) 

(sum 0) 

(x nil) 

(x-bar (first bar-point) ) ) 

( (null points) sum) 

(setf X (first (car points))) 

(setf sum (+ sum (* (- x x-bar) (- x x-bar) ) ) ) ) ) 
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efun get-constants (points) 

(let* ((bar-point (average-point points)) 

(coiranon-denominator (get -common-denominator points 
(al (get-al points bar-point common-denominator) ) 
(a2 (get-a2 points bar-point common-denominator) ) 
(aO (get-aO bar-point al) ) 

(a3 (get-a3 bar-point a2) ) 

(a4 (get-a4 points aO al a2 a3) ) ) 

(list aO al a2 a3 a4))) 



bar-point) ) 



efun get-footholds (legs) 

(do* ( (legs legs (cdr legs) ) 

(footholds nil) 

(a-leg nil) ) 

( (null legs) footholds) 

(setf a-leg (car legs) ) 

(if (leg-foothold a-leg) 

(setf footholds (cons (leg-foothold a-leg) footholds) ) ) ) ) 



efun get-yr (points a2 a3) 

(do* ( (points points (cdr points) ) 

(yr nil) 

(x nil) 

(y nil)) 

( (null points) (reverse yr) ) 

(setf X (first (car points) ) ) 

(setf y (second (car points) ) ) 

(setf yr (cons (- y a2 (* a3 x) ) yr) ) ) ) 



efun get-yr-bar (yr number-of-point s) 

(do ( (yr yr (cdr yr) ) 

(yr-bar 0)) 

( (null yr) (/ yr-bar number-of-point s) ) 
(setf yr-bar (+ yr-bar (first yr) ) ) ) ) 



efun get-zr (points aO al) 

(do* ( (points points (cdr points) ) 

(zr nil) 

(x nil) 

(z nil)) 

( (null points) (reverse zr) ) 

(setf X (first (car points) ) ) 

(setf z (third (car points) ) ) 

(setf zr (cons (- z aO (* al x) ) zr) ) ) ) 



efun get-zr-bar (zr number-of-point s) 

(do ( (zr zr (cdr zr) ) 

( zr-bar 0) ) 

( (null zr) (/ zr-bar number-of-point s) ) 
(setf zr-bar (+ zr-bar (first zr) ) ) ) ) 
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(defun make-plane-f rom-coef f icient (constants) 

(let* ( (aO (first constants) ) 

(al (second constants) ) 

(a2 (third constants) ) 

(a3 (fourth constants) ) 

(a4 (fifth constants) ) 

(a (- (* a4 a3) al) ) 

(b (- a4)) 

(c 1) 

(d (- (* a2 a4) aO) ) 

(unit-normal (normalize-vector (list a b c) ) ) 
(dis (/ d (magnitude (list a b c) ) ) ) ) 

(list unit-normal dis) ) ) 
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;; Mode: LISP; Syntax: Coinmon-lisp; Package: BODY 

I 

terrain-regulator flavor definition 

ief flavor terrain-regulator (body-rotate-rate-x body-rotate-rate-y 

body-trans-rate-z old-body-rotate -rate -x 
old-body- rot ate -rat e-y old-body- trans -rate- z 
gain min-height max-height 
etal eta2 min-eta max-eta) 

(regulator) 

: initable-instance-variables ) 



defmethod (init terrain-regulator) 

0 

(setf gain 5) 



(setf 


min-eta 0.0000001) 


; 0 


degree 


(setf 


max-eta 0.4363) 


; 25 


degrees 


(setf 


min-height 4.4) 


; 4.4 


feet 


(setf 


max-height 5.4) 


; 5.4 


feet 


(setf 


etal min-eta) 


; 0 


degree 


(setf 


eta2 0.5236) 


; 30 


degree 



(setf body-rotate-rate-x 0.0) 

(setf body-rotate-rate-y 0.0) 

(setf body-trans-rate-z 0.0) 

(list body-rotate-rate-x body-rotate-rate-y body-trans-rate-z) ) 



defmethod (do-terrain-regulation terrain-regulator) 
(k-gamma-delta-height) 

k- gamma- delta- height is ((k.x k.y k.z) gamma delta-height). 

(let* ( (k (first k-gamma-delta-height)) 

(gamma (second k-gamma-delta-height)) 

(delta-height (third k-gamma-delta-height) ) 
(body-rotate-rate-x-n (* gain (first k) gamma) ) 
(body-rotate-rate-y-n (* gain (second k) gamma) ) 
(body-trans-rate-z-n (* gain delta-height))) 

(setf body-rotate-rate-x 
(limitor self 

(filter self body-rotate-rate-x-n body-rotate-rate-x) 

0 . 1 )) 

(setf body-rotate-rate-y 
(limitor self 

(filter self body-rotate-rate-y-n body-rotate-rate-y) 

0 . 1 )) 

(setf body-trans-rate-z 
(limitor self 

(filter self body-trans-rate-z-n body-trans-rate-z) 

1 ))) 

(list body-rotate-rate-x body-rotate-rate-y body-trans-rate-z) ) 



defmethod (eta-function terrain-regulator) 
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(eta) 

(let ((slope (/ (- max-eta min-eta) (- eta2 etal) ) ) ) 

(+ min-eta (* slope (- eta etal))))) 



(defmethod (get-k-gamma-by-slope terrain-regulator) 

(plane H) 

(lef^ ( (plane-rpt-body (plane-transform plane H) ) 

(height (cadr plane-rpt-body) ) 

(eta (arc-cos (third (car plane) ) ) ) 
(k-gamma-desired-height nil) ) 

(setf k-gamima-desired-height 

(cond ( (< eta etal) (low-slope self plane) ) 

( (< eta eta2) (mid-slope self eta plane H) ) 
(T (high-slope self plane H) ) ) ) 

(list (first k-gamma-desired-height) 

(second k-gamma-desired-height) 

(- (third k-gamma-desired-height) height)))) 



(defmethod (height-function terrain-regulator) 

(eta) 

(let ( (slope (/ (- max-height min-height) (- eta2 etal) ) ) ) 

(- max-height (* slope (- eta etal))))) 



(defmethod (high-slope terrain-regulator) 

(plane H) 

(let* ( (plane-unit-normal (first plane) ) 

(a (first plane-unit-normal) ) 

(b (second plane-unit-normal) ) 

(m (sqrt (+ (* a a) (* b b) ) ) ) 

(desired-eta max-eta) 

(desired-height min-height) 

(desired-body-plane (list (list (* (/ a m) (sin desired-eta)) 

(* (/ b m) (sin desired-eta)) 

(cos desired-eta)) 0.0)) 

(desired-body-plane-in-body (plane-transform desired-body-plane H) ) 
(unit-normal-body-plane (first desired-body-plane-in-body) ) 

(al (first unit-normal-body-plane) ) 

(bl (second unit-normal-body-plane)) 

(cl (third unit-normal-body-plane) ) 

(ml (sqrt (+ (* al al) (* bl bl) ) ) ) 

(k (if (= ml 0) 

(list 000) 

(list (/ (- bl) ml) (/ al ml) 0))) 

(gamma (arc-cos cl) ) ) 

(list k gamima desired-height) ) ) 



(defmethod (limitor terrain-regulator) 
(vel max-vel) 

(if (>= (abs vel) max-vel) 

(if (> vel 0) 
max-vel 



(- max-vel) ) 
vel) ) 
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iefmethod (low-slope terrain-regulator) 

(plane) 

(let* ( (unit-normal (first plane) ) 

(a (first unit-normal) ) 

(b (second unit-normal) ) 

(c (third unit-normal) ) 

(m (sqrt (+ (* a a) (* b b) ) ) ) 

(k.a nil) 

(k.b nil) 

(gamma (arc-cos c) ) 

(desired-height max-height) ) 

(if (= m 0 . 0) 

(setf k.a 0.0 k.b 0.0) 

(setf k.a (/ (- b) m) k.b (/ am))) 

(list (list k.a k.b 0.0) gamma desired-height))) 



iefmethod (mid-slope terrain-regulator) 

I (eta plane H) 

I (let* ((plane-unit-normal (first plane)) 

(a (first plane-unit-normal) ) 

(b (second plane-unit -normal) ) 

(m (sqrt (+ (* a a) (* b b) ) ) ) 

(desired-eta (eta-function self eta) ) 

(desired-height (height-function self eta) ) 

(desired-body-plane (list (list (* (/ a m) (sin desired-eta) ) 

(* (/ b m) (sin desired-eta)) 

(cos desired-eta)) 0.0)) 

(desired-body-plane-in-body (plane-transform desired-body-plane H) ) 
(unit-normal-body-plane (first desired-body-plane-in-body) ) 

(al (first unit -normal-body-plane) ) 

(bl (second unit -normal-body-plane) ) 

(cl (third unit -normal-body-plane) ) 

(ml (sqrt (-f (* al al) (* bl bl) ) ) ) 

(k (if (= ml 0) 

(list 000) 

(list (/ (- bl) ml) (/ al ml) 0))) 

(gamma (arc-cos cl) ) ) 

(list k gamma desired-height) ) ) 



defmethod (regulate terrain-regulator) 

(estimated-support-plane H) 

(let ( (k-gamma (get-k-gamma-by-slope self estimated-support -plane H) ) ) 
(do-terrain-regulation self k-gamma) ) ) 



defmethod (restore terrain-regulator) 

0 

(setf body-rotate-rate-x old-body-rotate-rate-x) 

(setf body-rotate-rate-y old-body-rotate-rate-y ) 

(setf body-t rans-rate-z old-body-trans-rate-z) 

(list body-rotate-rate-x body-rotate-rate-y body-trans-rate-z) ) 



defmethod (save terrain-regulator) 

0 

(setf old-body-rotate-rate-x body-rotate-rate-x) 
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(setf old-body-rotate-rate-y body-rot ate-rate-y) 
(setf old-body-trans-rate-z body-t rans-rate-z) 

) 
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Mode: LISP; Syntax: Common-lisp; Package: LEG 



I 

J tkm-calculator flavor definition 



lef flavor tkm-calculator (working-volume owner) 

0 

: initable-instance-variables) 



iefmethod (init tkm-calculator) 
(leg-name) 

(cond ((equal leg-name 'legl) 
(setf working-volume 





' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-8.0832) 


( (0 


0.9397 


0.3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


-3.4167) 


( (0 


0.9397 


-0.3420) 


-2.569) ) ) ) ) 


( (equal 


leg-name 'leg2) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-8.0832) 


( (0 


0.9397 


0.3420) 


2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


-3.4167) 


( (0 


0.9397 


-0.3420) 


2.569))))) 


( (equal 


leg-name 'leg3) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


-2.2915) 


((0 


0.9397 


0.3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


2.2915) 


( (0 


0.9397 


-0.3420) 


-2.569))))) 


( (equal 


leg-name 'leg4) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


111 


0 


0) 


-2.2915) 


( (0 


0.9397 


0.3420) 


2.569) ) 




( ( (0 0 1) 5.7313) 


(d 


0 


0) 


2.2915) 


( (0 


0.9397 


-0.3420) 


2.569) )))) 


( (equal 


leg-name 'leg5) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


( (1 


0 


0) 


3.3332) 


( (0 


0.9397 


0.3420) 


-2.569) ) 




( ( (0 0 1) 5.7313) 


( (1 


0 


0) 


7.8332) 


( (0 


0.9397 


-0.3420) 


-2.569) ) ) ) ) 


( (equal 


leg-name 'leg6) 


















(setf 


working- volume 
' ( ( ( (0 0 1) 3.316) 


(d 


0 


0) 


3.3332) 


( (0 


0.9397 


0.3420) 


2.569) ) 




( ( (0 0 1) 5.7313) 


( d 


0 


0) 


7.8332) 


( (0 


0.9397 


-0.3420) 


2.569) ) ) ) ) 



) 

) 



defmethod (find-tkm tkm-calculator) 

(a-foothold body-trans-rate body-rotate-rate) 
a-foothold is based on body coordinate 
returns tkm 

(let* ( (leg-vel-rpt-body 

(get -leg-velocity 

a-foothold body-trans-rate body-rotate-rate) ) ) 
(get-tkm a-foothold leg-vel-rpt-body working-volume) ) ) 



defun get-distance (planes velocity leg-position) 
global function : plane-distance 
before start, make one plane list 

(do ( (planes (append (first planes) (second planes) ) (cdr planes) ) 
(a-tkm nil) 

(min-tkm 10000) ) 

( (null planes) min-tkm) 



tkm . lisp 



Wed Mar 30 15:28:17 1988 



2 



(setf a-tkm (plane-distance (car planes) velocity leg-position) ) 
(if a-tkm 

(if (and (> a-t)oa 0) (> min-tkm a-tkm) ) 

(setf min-t)oa a-tkm) ) ) ) ) 



(defun get-leg-velocity (pos-rpt-body body-trans-rate body-rotate-rate) 
; returns leg-velocity-wrt-body 

; = - ( body-trans-rate + body-rotate-rate X pos-rpt-body ) 

(vectsub '(000) 

(vectadd body-trans-rate 

(crossprod body-rotate-rate pos-rpt-body) ) ) ) 



(defun get-tkm (leg-pos-rpt-body velocity working-voliome) 

; global function : magnitude 

; outside w.v returns nil. If speed is near 0, then returns 1000.0. 

(if (in-side-voliame leg-pos-rpt-body working-volume) 

(let ( (speed (magnitude velocity) ) ) 

(if (< speed 1/1000) 

1000.0 

(/ (get-distance working-volume velocity leg-pos-rpt-body) speed) ) ) 

nil) ) 



(defun in-side-voliome (position planes) 

; planes ( (up front left) (back right bottom) ) 

(let* ( (positive-planes (first planes) ) 

(negative-planes (second planes) ) 

(inside-flag T) ) 

(dolist (a-plane positive-planes) 

(if (>= (plane-normal-distance a-plane position) 0) 
(setf inside-flag nil) ) ) 

(dolist (a-plane negative-planes) 

(if (<= (plane-normal-distance a-plane position) 
(setf inside-flag nil))) 
inside-flag) ) 



0 ) 
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.. Mode: LISP; Syntax: Coiranon-lisp; Package: VISION 

‘iefflavor vision-system (owner) 

! 

I ; initable-instance-variables) 



iefmethod 

) 

jiefmethod 

i ) 



(init vision-system) 
0 



(scanning vision-system) 
0 



iefmethod (permitted-cell vision-system) 
(t-cell) 

(permitted-cell graph-terrain t-cell) ) 



defmethod (terrain-point vision-system) 
(t-cell) 

, (terrain-point graph-terrain t-cell) ) 
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